Position Override created to allow position testing

rj_msgs/CMakeLists.txt

@@ -52,6 +52,7 @@ rosidl_generate_interfaces(
   msg/TeamInfo.msg
   msg/Trajectory.msg
   msg/WorldState.msg
+  msg/OverridePosition.msg
 
   # Communication
   msg/AgentRequest.msg

rj_msgs/msg/OverridePosition.msg

@@ -0,0 +1,6 @@
+# Contains two unsigned ints - The robot ID to override and the position
+# to set for that ID, based on the enum OverridingPositions,
+# which is contained in soccer/src/soccer/strategy/agent/position/overriding_positions.hpp
+
+uint32 robot_id
+uint32 overriding_position

soccer/src/soccer/strategy/agent/position/overriding_positions.hpp

@@ -0,0 +1,28 @@
+#pragma once
+#include <map>
+#include <string>
+#include <vector>
+
+namespace Strategy {
+/*
+    OverridingPositions refers to the positions that can be manually set in the UI.
+    Normally, all robots are set to Auto. If you want to add a new position, add a new value
+    to this enum before LENGTH, add a string to the overriding_position_labels vector in
+   main_window.hpp, and add a case to the check_for_position_override method in
+   RobotFactoryPosition.
+*/
+enum OverridingPositions {
+    AUTO,
+    OFFENSE,
+    DEFENSE,
+    FREE_KICKER,
+    PENALTY_PLAYER,
+    PENALTY_NON_KICKER,
+    SOLO_OFFENSE,
+    SMART_IDLE,
+    ZONER,
+    IDLE,
+    LENGTH,  // Do not remove
+};
+
+}  // namespace Strategy

soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp

@@ -15,6 +15,14 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id, "RobotFact
     } else {
         current_position_ = std::make_unique<Defense>(robot_id_);
     }
+
+    std::string node_name{"robot_factory_position_"};
+    _node = std::make_shared<rclcpp::Node>(node_name.append(std::to_string(robot_id_)));
+    test_play_sub_ = _node->create_subscription<rj_msgs::msg::OverridePosition>(
+        "override_position_for_robot", 1,
+        [this](const rj_msgs::msg::OverridePosition::SharedPtr msg) { test_play_callback(msg); });
+    _executor.add_node(_node);
+    _executor_thread = std::thread([this]() { _executor.spin(); });
 }
 
 std::optional<RobotIntent> RobotFactoryPosition::derived_get_task([
@@ -119,6 +127,14 @@ void RobotFactoryPosition::handle_ready() {
 }
 
 void RobotFactoryPosition::update_position() {
+    // if (test_play_position_ == Strategy::OverridingPositions::OFFENSE) {
+    //  SPDLOG_INFO("OFFENSE SELECTED");
+    // set_current_position<Offense>();
+    // return;
+    //}
+    bool manual_position_set = check_for_position_override();
+    if (manual_position_set) return;
+
     switch (current_play_state_.state()) {
         case PlayState::State::Playing: {
             // We just became regular playing.
@@ -338,4 +354,61 @@ std::string RobotFactoryPosition::get_current_state() {
     return current_position_->get_current_state();
 }
 
+void RobotFactoryPosition::test_play_callback(
+    const rj_msgs::msg::OverridePosition::SharedPtr message) {
+    if (message->robot_id == this->robot_id_ &&
+        message->overriding_position < Strategy::OverridingPositions::LENGTH) {
+        test_play_position_ =
+            static_cast<Strategy::OverridingPositions>(message->overriding_position);
+    }
+}
+
+/**
+ * Checks test_play_position_, which automatically updates when an override is set.
+ * If it is anything but auto, set the current position to that position and return true.
+ */
+bool RobotFactoryPosition::check_for_position_override() {
+    switch (test_play_position_) {
+        case Strategy::OverridingPositions::OFFENSE: {
+            set_current_position<Offense>();
+            return true;
+        }
+        case Strategy::OverridingPositions::DEFENSE: {
+            set_current_position<Defense>();
+            return true;
+        }
+        case Strategy::OverridingPositions::FREE_KICKER: {
+            set_current_position<FreeKicker>();
+            return true;
+        }
+        case Strategy::OverridingPositions::PENALTY_PLAYER: {
+            set_current_position<PenaltyPlayer>();
+            return true;
+        }
+        case Strategy::OverridingPositions::PENALTY_NON_KICKER: {
+            set_current_position<PenaltyNonKicker>();
+            return true;
+        }
+        case Strategy::OverridingPositions::SMART_IDLE: {
+            set_current_position<SmartIdle>();
+            return true;
+        }
+        case Strategy::OverridingPositions::SOLO_OFFENSE: {
+            set_current_position<SoloOffense>();
+            return true;
+        }
+        case Strategy::OverridingPositions::ZONER: {
+            set_current_position<Zoner>();
+            return true;
+        }
+        case Strategy::OverridingPositions::IDLE: {
+            set_current_position<Idle>();
+            return true;
+        }
+        default: {
+            return false;
+        }
+    }
+}
+
 }  // namespace strategy

soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp

@@ -10,17 +10,20 @@
 #include <rj_msgs/action/robot_move.hpp>
 
 #include "game_state.hpp"
+#include "overriding_positions.hpp"
 #include "planning/instant.hpp"
 #include "position.hpp"
 #include "rj_common/field_dimensions.hpp"
 #include "rj_common/time.hpp"
 #include "rj_constants/constants.hpp"
 #include "rj_geometry/geometry_conversions.hpp"
+#include "rj_msgs/msg/override_position.hpp"
 #include "strategy/agent/position/defense.hpp"
 #include "strategy/agent/position/free_kicker.hpp"
 #include "strategy/agent/position/goal_kicker.hpp"
 #include "strategy/agent/position/goalie.hpp"
 #include "strategy/agent/position/offense.hpp"
+#include "strategy/agent/position/overriding_positions.hpp"
 #include "strategy/agent/position/penalty_non_kicker.hpp"
 #include "strategy/agent/position/penalty_player.hpp"
 #include "strategy/agent/position/pivot_test.hpp"
@@ -109,6 +112,14 @@
 private:
     std::unique_ptr<Position> current_position_;
 
+    // subscription for test mode - Allows position overriding
+    rclcpp::Subscription<rj_msgs::msg::OverridePosition>::SharedPtr test_play_sub_;
+    rclcpp::executors::SingleThreadedExecutor _executor;
+    std::thread _executor_thread;
+    void test_play_callback(const rj_msgs::msg::OverridePosition::SharedPtr message);
+    Strategy::OverridingPositions test_play_position_{Strategy::OverridingPositions::AUTO};
+    rclcpp::Node::SharedPtr _node;
+
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
 
     bool am_closest_kicker();
@@ -153,6 +164,8 @@
             current_position_ = std::make_unique<Pos>(*current_position_);
         }
     }
+
+    bool check_for_position_override();
 };
 
 }  // namespace strategy

soccer/src/soccer/ui/main_window.cpp

@@ -17,6 +17,7 @@
 
 #include <rj_common/qt_utils.hpp>
 #include <rj_constants/topic_names.hpp>
+#include <rj_msgs/msg/override_position.hpp>
 #include <rj_protos/grSim_Packet.pb.h>
 #include <rj_protos/grSim_Replacement.pb.h>
 #include <ui/style_sheet_manager.hpp>
@@ -49,6 +50,7 @@ void calcMinimumWidth(QWidget* widget, const QString& text) {
     widget->setMinimumWidth(rect.width());
 }
 
+// TODO: Set up the UI objects and bind them to methods
 MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* parent)
     : QMainWindow(parent),
       _updateCount(0),
@@ -185,8 +187,19 @@ MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* par
     _set_game_settings = _node->create_client<rj_msgs::srv::SetGameSettings>(
         config_server::topics::kGameSettingsSrv);
 
+    test_play_pub_ =
+        _node->create_publisher<rj_msgs::msg::OverridePosition>("override_position_for_robot", 1);
+
     _executor.add_node(_node);
     _executor_thread = std::thread([this]() { _executor.spin(); });
+
+    // Connect up all the test position buttons and dropdowns to their listeners
+    for (int i = 0; i < kNumShells; ++i) {
+        robot_pos_selectors[i] =
+            findChild<QComboBox*>(QString::fromStdString("robotPosition_" + std::to_string(i)));
+        position_reset_buttons[i] =
+            findChild<QPushButton*>(QString::fromStdString("positionReset_" + std::to_string(i)));
+    }
 }
 
 void MainWindow::initialize() {
@@ -197,6 +210,8 @@ void MainWindow::initialize() {
         _ui.actionTeamYellow->trigger();
     }
 
+    populate_override_position_dropdowns();
+
     logFileChanged();
 
     // Initialize to ui defaults
@@ -1033,6 +1048,15 @@ void MainWindow::on_actionUse_Multiple_Joysticks_toggled(bool value) {
 
 void MainWindow::on_goalieID_currentIndexChanged(int value) {
     update_cache(_game_settings.request_goalie_id, value - 1, &_game_settings_valid);
+    // This is a convoluted scheme to figure out who the goalie is
+    // May not be necessary later
+    QString goalieNum = _ui.goalieID->currentText();
+    bool goalieNumIsInt{false};
+    int goalieInt = goalieNum.toInt(&goalieNumIsInt);
+    if (goalieNumIsInt)
+        setGoalieDropdown(goalieInt);
+    else
+        setGoalieDropdown(-1);
 }
 
 ////////////////
@@ -1146,3 +1170,154 @@ void MainWindow::updateDebugLayers(const LogFrame& frame) {
         _ui.debugLayers->sortItems();
     }
 }
+
+/**
+ * The following methods are event listeners for the manual position assignments.
+ * TODO: implement all of these
+ */
+void MainWindow::on_positionReset_0_clicked() { onResetButtonClicked(0); }
+
+void MainWindow::on_positionReset_1_clicked() { onResetButtonClicked(1); }
+
+void MainWindow::on_positionReset_2_clicked() { onResetButtonClicked(2); }
+
+void MainWindow::on_positionReset_3_clicked() { onResetButtonClicked(3); }
+
+void MainWindow::on_positionReset_4_clicked() { onResetButtonClicked(4); }
+
+void MainWindow::on_positionReset_5_clicked() { onResetButtonClicked(5); }
+
+void MainWindow::on_positionReset_6_clicked() { onResetButtonClicked(6); }
+
+void MainWindow::on_positionReset_7_clicked() { onResetButtonClicked(7); }
+
+void MainWindow::on_positionReset_8_clicked() { onResetButtonClicked(8); }
+
+void MainWindow::on_positionReset_9_clicked() { onResetButtonClicked(9); }
+
+void MainWindow::on_positionReset_10_clicked() { onResetButtonClicked(10); }
+
+void MainWindow::on_positionReset_11_clicked() { onResetButtonClicked(11); }
+
+void MainWindow::on_positionReset_12_clicked() { onResetButtonClicked(12); }
+
+void MainWindow::on_positionReset_13_clicked() { onResetButtonClicked(13); }
+
+void MainWindow::on_positionReset_14_clicked() { onResetButtonClicked(14); }
+
+void MainWindow::on_positionReset_15_clicked() { onResetButtonClicked(15); }
+
+void MainWindow::on_robotPosition_0_currentIndexChanged(int value) {
+    onPositionDropdownChanged(0, value);
+}
+
+void MainWindow::on_robotPosition_1_currentIndexChanged(int value) {
+    onPositionDropdownChanged(1, value);
+}
+
+void MainWindow::on_robotPosition_2_currentIndexChanged(int value) {
+    onPositionDropdownChanged(2, value);
+}
+
+void MainWindow::on_robotPosition_3_currentIndexChanged(int value) {
+    onPositionDropdownChanged(3, value);
+}
+
+void MainWindow::on_robotPosition_4_currentIndexChanged(int value) {
+    onPositionDropdownChanged(4, value);
+}
+
+void MainWindow::on_robotPosition_5_currentIndexChanged(int value) {
+    onPositionDropdownChanged(5, value);
+}
+
+void MainWindow::on_robotPosition_6_currentIndexChanged(int value) {
+    onPositionDropdownChanged(6, value);
+}
+
+void MainWindow::on_robotPosition_7_currentIndexChanged(int value) {
+    onPositionDropdownChanged(7, value);
+}
+
+void MainWindow::on_robotPosition_8_currentIndexChanged(int value) {
+    onPositionDropdownChanged(8, value);
+}
+
+void MainWindow::on_robotPosition_9_currentIndexChanged(int value) {
+    onPositionDropdownChanged(9, value);
+}
+
+void MainWindow::on_robotPosition_10_currentIndexChanged(int value) {
+    onPositionDropdownChanged(10, value);
+}
+
+void MainWindow::on_robotPosition_11_currentIndexChanged(int value) {
+    onPositionDropdownChanged(11, value);
+}
+
+void MainWindow::on_robotPosition_12_currentIndexChanged(int value) {
+    onPositionDropdownChanged(12, value);
+}
+
+void MainWindow::on_robotPosition_13_currentIndexChanged(int value) {
+    onPositionDropdownChanged(13, value);
+}
+
+void MainWindow::on_robotPosition_14_currentIndexChanged(int value) {
+    onPositionDropdownChanged(14, value);
+}
+
+void MainWindow::on_robotPosition_15_currentIndexChanged(int value) {
+    onPositionDropdownChanged(15, value);
+}
+
+void MainWindow::onResetButtonClicked(int robot) {
+    rj_msgs::msg::OverridePosition message;
+    message.robot_id = robot;
+    message.overriding_position = 0;
+
+    test_play_pub_->publish(message);
+    position_reset_buttons.at(robot)->setEnabled(false);
+    robot_pos_selectors.at(robot)->setCurrentIndex(0);
+}
+
+void MainWindow::onPositionDropdownChanged(int robot, int position_number) {
+    // This is a convoluted scheme to figure out who the goalie is
+    // May not be necessary later
+    QString goalieNum = _ui.goalieID->currentText();
+    bool goalieNumIsInt{false};
+    int goalieInt = goalieNum.toInt(&goalieNumIsInt);
+    if (!goalieNumIsInt || goalieInt != robot) {
+        rj_msgs::msg::OverridePosition message;
+        message.robot_id = robot;
+        message.overriding_position = position_number;
+
+        test_play_pub_->publish(message);
+        if (position_number != 0) {
+            position_reset_buttons.at(robot)->setEnabled(true);
+        } else {
+            position_reset_buttons.at(robot)->setEnabled(false);
+        }
+    }
+}
+
+void MainWindow::setGoalieDropdown(int robot) {
+    for (int i = 0; i < robot_pos_selectors.size(); ++i) {
+        if (i != robot) {
+            robot_pos_selectors.at(i)->setEnabled(true);
+        } else {
+            robot_pos_selectors.at(i)->setCurrentIndex(0);
+            robot_pos_selectors.at(i)->setEnabled(false);
+            onResetButtonClicked(i);
+        }
+    }
+}
+
+void MainWindow::populate_override_position_dropdowns() {
+    for (int i = 0; i < kNumShells; i++) {
+        robot_pos_selectors[i]->clear();
+        for (int j = 0; j < overriding_position_labels.size(); j++) {
+            robot_pos_selectors[i]->addItem(QString::fromStdString(overriding_position_labels[j]));
+        }
+    }
+}