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,5 @@
+# Contains unsigned int overriding_position -  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 overriding_position

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

@@ -0,0 +1,27 @@
+#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, add a string to the overriding_position_labels vector in
+   main_window.hpp, and add a case to the set_position_override_if_requested method in
+   RobotFactoryPosition.
+*/
+enum OverridingPositions {
+    AUTO,
+    OFFENSE,
+    DEFENSE,
+    FREE_KICKER,
+    PENALTY_PLAYER,
+    PENALTY_NON_KICKER,
+    SOLO_OFFENSE,
+    SMART_IDLE,
+    ZONER,
+    IDLE,
+};
+
+}  // namespace Strategy

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

@@ -15,6 +15,14 @@
     } 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/robot_" + std::to_string(robot_id_), 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([
@@ -118,7 +126,12 @@
     }
 }
 
 void RobotFactoryPosition::update_position() {
+    bool manual_position_set = set_position_override_if_requested();
+    if (manual_position_set) {
+        return;
+    }
+
     switch (current_play_state_.state()) {
         case PlayState::State::Playing: {
             // We just became regular playing.
@@ -338,4 +351,58 @@
     return current_position_->get_current_state();
 }
 
+void RobotFactoryPosition::test_play_callback(
+    const rj_msgs::msg::OverridePosition::SharedPtr message) {
+    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::set_position_override_if_requested() {
+    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,18 +10,21 @@
 #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/line.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"
@@ -110,6 +113,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();
@@ -154,6 +165,8 @@
             current_position_ = std::make_unique<Pos>(*current_position_);
         }
     }
+
+    bool set_position_override_if_requested();
 };
 
 }  // namespace strategy

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

@@ -123,7 +123,7 @@ std::optional<RobotIntent> SoloOffense::state_to_task(RobotIntent intent) {
             planning::LinearMotionInstant target{calculate_best_shot()};
             // planning::LinearMotionInstant target{last_world_state_->ball.position};
             auto kick_cmd =
-                planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true};
+                planning::MotionCommand{"line_kick", target, planning::FaceTarget{}, true};
             intent.motion_command = kick_cmd;
             intent.shoot_mode = RobotIntent::ShootMode::KICK;
             intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;