merged remove-coach-node

launch/soccer.launch.py

@@ -182,14 +182,6 @@ def generate_launch_description():
             ),
             # spawn internal_ref/external_ref based on internal_ref
             # LaunchArgument
-            Node(
-                condition=IfCondition(PythonExpression(["not ", use_manual_control])),
-                package="rj_robocup",
-                executable="coach_node",
-                output="screen",
-                parameters=[param_config_filepath],
-                on_exit=Shutdown(),
-            ),
             Node(
                 condition=IfCondition(PythonExpression(["not ", use_manual_control])),
                 package="rj_robocup",

soccer/src/soccer/planning/planner_node.cpp

@@ -244,9 +244,40 @@ 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 Overrides
+    switch (play_state.state()) {
+        case PlayState::State::Halt:
+            min_dist_from_ball = 0;
+            max_robot_speed = 0;
+            max_dribbler_speed = 0;
+            break;
+        case PlayState::State::Stop:
+            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.
+
+            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 +428,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

@@ -24,6 +24,10 @@ AgentActionClient::AgentActionClient(int r_id)
         ::vision_filter::topics::kWorldStateTopic, 1,
         [this](rj_msgs::msg::WorldState::SharedPtr msg) { world_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); });
@@ -77,6 +81,14 @@ void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::Sha
     last_world_state_ = std::move(world_state);
 }
 
+void AgentActionClient::play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg) {
+    if (current_position_ == nullptr) {
+        return;
+    }
+
+    current_position_->update_play_state(*msg);
+}
+
 void AgentActionClient::field_dimensions_callback(
     const rj_msgs::msg::FieldDimensions::SharedPtr& msg) {
     if (current_position_ == nullptr) {

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

@@ -53,13 +53,16 @@ class AgentActionClient : public rclcpp::Node {
 private:
     // ROS pub/subs
     rclcpp::Subscription<rj_msgs::msg::WorldState>::SharedPtr world_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_;
     rclcpp::Subscription<rj_msgs::msg::GameSettings>::SharedPtr game_settings_sub_;
     // TODO(Kevin): communication module pub/sub here (e.g. passing)
 
     // 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 field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg);
     void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg);
     void game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg);

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

@@ -8,7 +8,9 @@ Offense::Offense(int r_id) : Position(r_id) {
 }
 
 std::optional<RobotIntent> Offense::derived_get_task(RobotIntent intent) {
+    SPDLOG_INFO("MY ID: {} in offense derived task!\n", robot_id_);
     current_state_ = update_state();
+    SPDLOG_INFO("My current offense state is {}", current_state_);
     return state_to_task(intent);
 }
 

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

@@ -46,6 +46,12 @@ bool Position::check_goal_canceled() {
     return false;
 }
 
+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);
+}
+
 void Position::update_alive_robots(std::vector<u_int8_t> alive_robots) {
     alive_robots_ = alive_robots;
 

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

@@ -67,9 +67,11 @@ class Position {
      * https://www.sandordargo.com/blog/2022/08/24/tmp-and-nvi
      */
 
-    std::optional<RobotIntent> get_task(WorldState& world_state, FieldDimensions& field_dimensions);
+    virtual std::optional<RobotIntent> get_task(WorldState& world_state, FieldDimensions& field_dimensions);
 
     // communication with AC
+    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();
 
@@ -218,6 +220,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;
 

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

@@ -14,9 +14,21 @@ if (robot_id_ == 0) {
 
 }
 
+std::optional<RobotIntent> RobotFactoryPosition::get_task(WorldState& ws, FieldDimensions& fd) {
+    return current_position_->get_task(ws, fd);
+}
+
 std::optional<RobotIntent> RobotFactoryPosition::derived_get_task(RobotIntent intent) {
-    // This is where the current_position can be reassigned based on the PlayState
-    return current_position_->get_task(*last_world_state_, field_dimensions_);
+    // This is where the current_position can be reassigned based on the
+    // PlayState
+
+
+    SPDLOG_ERROR("This should not be called");
+    return std::nullopt;
+
+    // no longer called
+
+    // return current_position_->get_task(*last_world_state_, field_dimensions_);
 }
 
 void RobotFactoryPosition::receive_communication_response(communication::AgentPosResponseWrapper response) {

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

@@ -37,6 +37,8 @@ class RobotFactoryPosition : public Position {
     RobotFactoryPosition(int r_id);
     ~RobotFactoryPosition() override = default;
 
+    std::optional<RobotIntent> get_task(WorldState&, FieldDimensions&) override;
+
     void receive_communication_response(communication::AgentPosResponseWrapper response) override;
     communication::PosAgentResponseWrapper receive_communication_request(
         communication::AgentPosRequestWrapper request) override;