Remove coach node

soccer/src/soccer/planning/planner_node.cpp

@@ -244,9 +244,52 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) {
     const auto* world_state = global_state_.world_state();
     const auto goalie_id = global_state_.goalie_id();
     const auto play_state = global_state_.play_state();
-    const auto min_dist_from_ball = global_state_.coach_state().global_override.min_dist_from_ball;
-    const auto max_robot_speed = global_state_.coach_state().global_override.max_speed;
-    const auto max_dribbler_speed = global_state_.coach_state().global_override.max_dribbler_speed;
+
+    float min_dist_from_ball{};
+    float max_robot_speed{};
+    float max_dribbler_speed{};
+
+    // Global Override code
+    // rj_msgs::msg::GlobalOverride global_override;
+    switch (play_state.state()) {
+        case PlayState::State::Halt:
+            // global_override.max_speed = 0;
+            // global_override.min_dist_from_ball = 0;
+            // global_override.max_dribbler_speed = 0;
+
+            min_dist_from_ball = 0;
+            max_robot_speed = 0;
+            max_dribbler_speed = 0;
+            break;
+        case PlayState::State::Stop:
+            // global_override.max_speed = 1.5;
+            // global_override.min_dist_from_ball = 0.5;
+            // global_override.max_dribbler_speed = 0;
+
+            min_dist_from_ball = 0.5;
+            max_robot_speed = 1.5;
+            max_dribbler_speed = 0;
+            break;
+        case PlayState::State::Playing:
+        default:
+            // Unbounded speed. Setting to -1 or 0 crashes planner, so use large number
+            // instead.
+            // global_override.max_speed = 10.0;
+            // global_override.min_dist_from_ball = 0;
+            // global_override.max_dribbler_speed = 255;
+
+            min_dist_from_ball = 0;
+            max_robot_speed = 10.0;
+            max_dribbler_speed = 255;
+            break;
+    }
+
+    // publish new necessary information
+
+    // const auto min_dist_from_ball = global_override.min_dist_from_ball;
+    // const auto max_robot_speed = global_override.max_speed;
+    // const auto max_dribbler_speed = global_override.max_dribbler_speed;
+
     const auto& robot = world_state->our_robots.at(robot_id_);
     const auto start = RobotInstant{robot.pose, robot.velocity, robot.timestamp};
 
@@ -397,4 +440,6 @@ bool PlannerForRobot::is_done() const {
     return current_path_planner_->is_done();
 }
 
+// void PlannerForRobot::calcGlobalOverrides
+
 }  // namespace planning

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

@@ -28,6 +28,10 @@
         topics::kCoachStateTopic, 1,
         [this](rj_msgs::msg::CoachState::SharedPtr msg) { coach_state_callback(msg); });
 
+    play_state_sub_ = create_subscription<rj_msgs::msg::PlayState>(
+        ::referee::topics::kPlayStateTopic, 10,
+        [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); });
+
     field_dimensions_sub_ = create_subscription<rj_msgs::msg::FieldDimensions>(
         "config/field_dimensions", 1,
         [this](rj_msgs::msg::FieldDimensions::SharedPtr msg) { field_dimensions_callback(msg); });
@@ -95,12 +99,20 @@
     last_world_state_ = std::move(world_state);
 }
 
-void AgentActionClient::coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg) {
+void AgentActionClient::play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg) {
     if (current_position_ == nullptr) {
         return;
     }
 
-    current_position_->update_coach_state(*msg);
+    current_position_->update_play_state(*msg);
+}
+
+void AgentActionClient::coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg) {
+    // if (current_position_ == nullptr) {
+    //     return;
+    // }
+
+    // current_position_->update_coach_state(*msg);
 }
 
 void AgentActionClient::field_dimensions_callback(

soccer/src/soccer/strategy/agent/agent_action_client.hpp

@@ -56,6 +56,7 @@ class AgentActionClient : public rclcpp::Node {
     // ROS pub/subs
     rclcpp::Subscription<rj_msgs::msg::WorldState>::SharedPtr world_state_sub_;
     rclcpp::Subscription<rj_msgs::msg::CoachState>::SharedPtr coach_state_sub_;
+    rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;
     rclcpp::Subscription<rj_msgs::msg::PositionAssignment>::SharedPtr positions_sub_;
     rclcpp::Subscription<rj_msgs::msg::FieldDimensions>::SharedPtr field_dimensions_sub_;
     rclcpp::Subscription<rj_msgs::msg::AliveRobots>::SharedPtr alive_robots_sub_;
@@ -64,6 +65,7 @@ class AgentActionClient : public rclcpp::Node {
 
     // callbacks for subs
     void world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg);
+    void play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg);
     void coach_state_callback(const rj_msgs::msg::CoachState::SharedPtr& msg);
     void field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg);
     void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg);

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

@@ -56,6 +56,8 @@ void Position::update_coach_state(rj_msgs::msg::CoachState msg) {
      * match_state_, our_possession_); */
 }
 
+void Position::update_play_state(rj_msgs::msg::PlayState msg) { current_play_state_ = msg; }
+
 void Position::update_field_dimensions(FieldDimensions field_dims) {
     field_dimensions_ = std::move(field_dims);
 }

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

@@ -71,6 +71,7 @@ class Position {
     // communication with AC
     void update_world_state(WorldState world_state);
     void update_coach_state(rj_msgs::msg::CoachState coach_state);
+    void update_play_state(rj_msgs::msg::PlayState msg);
     void update_field_dimensions(FieldDimensions field_dimensions);
     void update_alive_robots(std::vector<u_int8_t> alive_robots);
     const std::string get_name();
@@ -220,6 +221,7 @@ class Position {
     // (if so match world_state below)
     bool our_possession_{};
     rj_msgs::msg::GlobalOverride global_override_{};
+    rj_msgs::msg::PlayState current_play_state_;
 
     FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions;