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_)));
+    override_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,9 @@ void RobotFactoryPosition::handle_ready() {
 }
 
 void RobotFactoryPosition::update_position() {
+    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 +349,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) {
+        override_play_position_ =
+            static_cast<Strategy::OverridingPositions>(message->overriding_position);
+    }
+}
+
+/**
+ * Checks override_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 (override_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 override_play_sub_;
+    rclcpp::executors::SingleThreadedExecutor _executor;
+    std::thread _executor_thread;
+    void test_play_callback(const rj_msgs::msg::OverridePosition::SharedPtr message);
+    Strategy::OverridingPositions override_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