fix alive_robots and goalie_id propagation

rj_constants/include/rj_constants/topic_names.hpp

@@ -99,7 +99,7 @@ constexpr auto kMotionControlParamModule = "motion_control";
 
 namespace radio::topics {
 
-constexpr auto kAliveRobotsTopic {"radio/alive_robots"};
+constexpr auto kAliveRobotsTopic{"radio/alive_robots"};
 
 static inline std::string robot_status_topic(int robot_id) {
     return "radio/robot_status/robot_" + std::to_string(robot_id);

soccer/src/soccer/planning/global_state.cpp

@@ -20,7 +20,7 @@ GlobalState::GlobalState(rclcpp::Node* node) {
             last_game_settings_ = rj_convert::convert_from_ros(*settings);
         });
     goalie_sub_ = node->create_subscription<rj_msgs::msg::Goalie>(
-        referee::topics::kGoalieTopic, rclcpp::QoS(1),
+        referee::topics::kGoalieTopic, rclcpp::QoS(1).transient_local(),
         [this](rj_msgs::msg::Goalie::SharedPtr goalie) {  // NOLINT
             auto lock = std::lock_guard(last_goalie_id_mutex_);
             last_goalie_id_ = goalie->goalie_id;

soccer/src/soccer/planning/planner/plan_request.hpp

@@ -39,8 +39,8 @@ struct PlanRequest {
           virtual_obstacles(std::move(virtual_obstacles)),
           planned_trajectories(planned_trajectories),
           shell_id(shell_id),
-          priority(priority),
           world_state(world_state),
+          priority(priority),
           play_state(play_state),
           debug_drawer(debug_drawer),
           ball_sense(ball_sense),

soccer/src/soccer/radio/sim_radio.cpp

@@ -98,16 +98,15 @@ SimRadio::SimRadio(bool blue_team)
     for (uint8_t robot_id = 0; robot_id < 6; robot_id++) {
         alive_robots_[robot_id] = true;
     }
-    alive_robots_pub_ = this->create_publisher<rj_msgs::msg::AliveRobots>(
-        topics::kAliveRobotsTopic, rclcpp::QoS(1));
+    alive_robots_pub_ = this->create_publisher<rj_msgs::msg::AliveRobots>(topics::kAliveRobotsTopic,
+                                                                          rclcpp::QoS(1));
 
     alive_robots_timer_ = create_wall_timer(std::chrono::milliseconds(500), [this]() {
         rj_msgs::msg::AliveRobots alive_message{};
         alive_message.alive_robots = alive_robots_;
         publish_alive_robots(alive_message);
     });
 
-
     buffer_.resize(1024);
     start_receive();
 

soccer/src/soccer/strategy/agent/agent_action_client.cpp

@@ -13,11 +13,12 @@ AgentActionClient::AgentActionClient() : AgentActionClient(0) {
 }
 
 AgentActionClient::AgentActionClient(int r_id)
-    : robot_id_(r_id),
-      rclcpp::Node(fmt::format("agent_{}_action_client_node", r_id),
+    : rclcpp::Node(::fmt::format("agent_{}_action_client_node", r_id),
                    rclcpp::NodeOptions{}
                        .automatically_declare_parameters_from_overrides(true)
-                       .allow_undeclared_parameters(true)) {
+                       .allow_undeclared_parameters(true)),
+      current_position_{std::make_unique<RobotFactoryPosition>(r_id)},
+      robot_id_{r_id} {
     // create a ptr to ActionClient
     client_ptr_ = rclcpp_action::create_client<RobotMove>(this, "robot_move");
 
@@ -42,7 +43,7 @@ AgentActionClient::AgentActionClient(int r_id)
         [this](rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); });
 
     goalie_id_sub_ = create_subscription<rj_msgs::msg::Goalie>(
-        ::referee::topics::kGoalieTopic, 1,
+        ::referee::topics::kGoalieTopic, rclcpp::QoS(1).transient_local(),
         [this](rj_msgs::msg::Goalie::SharedPtr msg) { goalie_id_callback(msg->goalie_id); });
 
     robot_communication_srv_ = create_service<rj_msgs::srv::AgentCommunication>(
@@ -52,9 +53,6 @@ AgentActionClient::AgentActionClient(int r_id)
             receive_communication_callback(request, response);
         });
 
-    // Default Positions with the Position class
-    current_position_ = std::make_unique<RobotFactoryPosition>(robot_id_);
-
     // Create clients
     for (size_t i = 0; i < kNumShells; i++) {
         robot_communication_cli_[i] =

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

@@ -4,6 +4,8 @@ namespace strategy {
 
 Defense::Defense(int r_id) : Position(r_id, "Defense") {}
 
+Defense::Defense(const Position& other) : Position{other} { position_name_ = "Defense"; }
+
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
     return state_to_task(intent);
@@ -113,7 +115,7 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             Waller waller{waller_id_, (int)walling_robots_.size()};
             return waller.get_task(intent, last_world_state_, this->field_dimensions_);
         }
-    } else if (current_state_ = FACING) {
+    } else if (current_state_ == FACING) {
         rj_geometry::Point robot_position =
             last_world_state_->get_robot(true, robot_id_).pose.position();
         auto current_location_instant =
@@ -195,7 +197,7 @@ void Defense::send_leave_wall_request() {
 
 communication::JoinWallResponse Defense::handle_join_wall_request(
     communication::JoinWallRequest join_request) {
-    for (int i = 0; i < walling_robots_.size(); i++) {
+    for (size_t i = 0; i < walling_robots_.size(); i++) {
         if (walling_robots_[i] == join_request.robot_id) {
             break;
         } else if (walling_robots_[i] > join_request.robot_id) {
@@ -234,7 +236,7 @@ communication::Acknowledge Defense::handle_leave_wall_request(
 }
 
 void Defense::handle_join_wall_response(communication::JoinWallResponse join_response) {
-    for (int i = 0; i < walling_robots_.size(); i++) {
+    for (size_t i = 0; i < walling_robots_.size(); i++) {
         if (walling_robots_[i] == join_response.robot_id) {
             return;
         } else if (walling_robots_[i] > join_response.robot_id) {

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

@@ -28,6 +28,7 @@ class Defense : public Position {
 public:
     Defense(int r_id);
     ~Defense() override = default;
+    Defense(const Position& other);
 
     void receive_communication_response(communication::AgentPosResponseWrapper response) override;
     communication::PosAgentResponseWrapper receive_communication_request(
@@ -42,8 +43,6 @@ class Defense : public Position {
     void revive() override;
 
 private:
-    int move_ct_ = 0;
-
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
      *  to do based on the game situation. The method will continuously look to assign

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

@@ -4,6 +4,10 @@ namespace strategy {
 
 Offense::Offense(int r_id) : Position{r_id, "Offense"}, seeker_{r_id} {}
 
+Offense::Offense(const Position& other) : Position{other}, seeker_{robot_id_} {
+    position_name_ = "Offense";
+}
+
 std::optional<RobotIntent> Offense::derived_get_task(RobotIntent intent) {
     // Get next state, and if different, reset clock
     State new_state = next_state();

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

@@ -28,7 +28,7 @@ class Offense : public Position {
 public:
     Offense(int r_id);
     ~Offense() override = default;
-
+    Offense(const Position& other);
     communication::PosAgentResponseWrapper receive_communication_request(
         communication::AgentPosRequestWrapper request) override;
 

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

@@ -7,7 +7,7 @@ namespace strategy {
 Position::Position(int r_id) : robot_id_(r_id) {}
 
 Position::Position(int r_id, std::string position_name)
-    : robot_id_{r_id}, position_name_{std::move(position_name)} {};
+    : position_name_{std::move(position_name)}, robot_id_{r_id} {};
 
 std::optional<RobotIntent> Position::get_task(WorldState& world_state,
                                               FieldDimensions& field_dimensions) {

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

@@ -55,6 +55,7 @@ class Position {
 public:
     Position(int r_id);
     virtual ~Position() = default;
+    Position(const Position& other) = default;
 
     /**
      * @brief return a RobotIntent to be sent to PlannerNode by AC; nullopt

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

@@ -25,7 +25,7 @@ std::optional<RobotIntent> RobotFactoryPosition::get_task(WorldState& world_stat
     using RobotPos = std::pair<int, double>;  // (robotId, yPosition)
 
     std::vector<RobotPos> robots_copy;
-    for (int i = 0; i < kNumShells; i++) {
+    for (int i = 0; i < static_cast<int>(kNumShells); i++) {
         // Ignore goalie
         if (i == goalie_id_) {
             continue;
@@ -50,35 +50,32 @@ std::optional<RobotIntent> RobotFactoryPosition::get_task(WorldState& world_stat
     // Assigning new position
     // Checking whether we have possesion or if the ball is on their half (using 1.99 to avoid
     // rounding issues on midline)
-    if (our_possession_ ||
-        world_state.ball.position.y() > field_dimensions.length() / 1.99) {
+    if (our_possession_ || world_state.ball.position.y() > field_dimensions.center_field_loc().y() - kBallDiameter) {
         // Offensive mode
         // Closest 2 robots on defense, rest on offense
         if (i <= 1) {
             if (current_position_->get_name() != "Defense") {
-                current_position_ = std::make_unique<Defense>(robot_id_);
+                current_position_ = std::make_unique<Defense>(*current_position_);
             }
         } else {
             if (current_position_->get_name() != "Offense") {
-                current_position_ = std::make_unique<Offense>(robot_id_);
+                current_position_ = std::make_unique<Offense>(*current_position_);
             }
         }
     } else {
         // Defensive mode
         // Closest 4 robots on defense, rest on offense
         if (i <= 3) {
             if (current_position_->get_name() != "Defense") {
-                current_position_ = std::make_unique<Defense>(robot_id_);
+                current_position_ = std::make_unique<Defense>(*current_position_);
             }
         } else {
             if (current_position_->get_name() != "Offense") {
-                current_position_ = std::make_unique<Offense>(robot_id_);
+                current_position_ = std::make_unique<Offense>(*current_position_);
             }
         }
     }
 
-    SPDLOG_INFO("ROBOT {} POSITION {}", robot_id_, current_position_->get_name())
-
     return current_position_->get_task(world_state, field_dimensions);
 }
 

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

@@ -80,6 +80,11 @@ class RobotFactoryPosition : public Position {
 
     void set_goal_canceled() override { current_position_->set_goal_canceled(); }
 
+    void set_goalie_id(int goalie_id) override {
+        Position::set_goalie_id(goalie_id);
+        current_position_->set_goalie_id(goalie_id);
+    }
+
     void send_direct_pass_request(std::vector<u_int8_t> target_robots) override {
         current_position_->send_direct_pass_request(target_robots);
     }
@@ -97,8 +102,6 @@ class RobotFactoryPosition : public Position {
         current_position_->send_pass_confirmation(target_robot);
     }
 
-    void set_goalie_id(int goalie_id) override { current_position_->set_goalie_id(goalie_id); }
-
 private:
     std::unique_ptr<Position> current_position_;