Made different topic for each robot id's overriding position

RoboJackets · Jan 27, 2025 · 5ca24d4 · 5ca24d4
1 parent d476111
commit 5ca24d4
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 13 deletions.
diff --git a/rj_msgs/msg/OverridePosition.msg b/rj_msgs/msg/OverridePosition.msg
@@ -2,5 +2,4 @@
 # 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
diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp
@@ -19,7 +19,7 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id, "RobotFact
     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,
+        "override_position_for_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(); });
@@ -353,10 +353,7 @@ std::string RobotFactoryPosition::get_current_state() {
 
 void RobotFactoryPosition::test_play_callback(
     const rj_msgs::msg::OverridePosition::SharedPtr message) {
-    if (message->robot_id == this->robot_id_) {
-        override_play_position_ =
-            static_cast<Strategy::OverridingPositions>(message->overriding_position);
-    }
+    override_play_position_ = static_cast<Strategy::OverridingPositions>(message->overriding_position);
 }
 
 /**

diff --git a/soccer/src/soccer/ui/main_window.cpp b/soccer/src/soccer/ui/main_window.cpp
@@ -188,8 +188,12 @@ MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* par
     _set_game_settings = _node->create_client<rj_msgs::srv::SetGameSettings>(
         config_server::topics::kGameSettingsSrv);
 
-    override_play_pub_ =
-        _node->create_publisher<rj_msgs::msg::OverridePosition>("override_position_for_robot", 1);
+    // Publishers to signal when a manual position override is occurring
+    for (int i = 0; i < 16; ++i) {
+        override_play_pubs_.push_back(
+            _node->create_publisher<rj_msgs::msg::OverridePosition>("override_position_for_robot_" + std::to_string(i), 1)
+        );
+    }
 
     _executor.add_node(_node);
     _executor_thread = std::thread([this]() { _executor.spin(); });
@@ -1278,21 +1282,19 @@ void MainWindow::on_robotPosition_15_currentIndexChanged(int value) {
 
 void MainWindow::onResetButtonClicked(int robot) {
     rj_msgs::msg::OverridePosition message;
-    message.robot_id = robot;
     message.overriding_position = 0;
 
-    override_play_pub_->publish(message);
+    override_play_pubs_.at(robot)->publish(message);
     position_reset_buttons.at(robot)->setEnabled(false);
     robot_pos_selectors.at(robot)->setCurrentIndex(0);
 }
 
 void MainWindow::onPositionDropdownChanged(int robot, int position_number) {
     if (current_goalie_num_ != robot) {
         rj_msgs::msg::OverridePosition message;
-        message.robot_id = robot;
         message.overriding_position = position_number;
 
-        override_play_pub_->publish(message);
+        override_play_pubs_.at(robot)->publish(message);
         if (position_number != 0) {
             position_reset_buttons.at(robot)->setEnabled(true);
         } else {

diff --git a/soccer/src/soccer/ui/main_window.hpp b/soccer/src/soccer/ui/main_window.hpp
@@ -297,7 +297,7 @@ private Q_SLOTS:
     rclcpp::Node::SharedPtr _node;
     rclcpp::Client<rj_msgs::srv::QuickCommands>::SharedPtr _quick_commands_srv;
     rclcpp::Client<rj_msgs::srv::SetGameSettings>::SharedPtr _set_game_settings;
-    rclcpp::Publisher<rj_msgs::msg::OverridePosition>::SharedPtr override_play_pub_;
+    std::vector<rclcpp::Publisher<rj_msgs::msg::OverridePosition>::SharedPtr> override_play_pubs_ {};
     rclcpp::executors::SingleThreadedExecutor _executor;
     std::thread _executor_thread;