Remove Defense obstacles from coach node

soccer/src/soccer/planning/planner_node.hpp

@@ -12,6 +12,7 @@
 #include <rj_constants/topic_names.hpp>
 #include <rj_msgs/action/robot_move.hpp>
 #include <rj_msgs/msg/coach_state.hpp>
+#include <rj_msgs/msg/detail/field_dimensions__struct.hpp>
 #include <rj_msgs/msg/goalie.hpp>
 #include <rj_msgs/msg/manipulator_setpoint.hpp>
 #include <rj_msgs/msg/robot_status.hpp>
@@ -44,6 +45,8 @@
             referee::topics::kPlayStateTopic, rclcpp::QoS(1),
             [this](rj_msgs::msg::PlayState::SharedPtr state) {  // NOLINT
                 last_play_state_ = rj_convert::convert_from_ros(*state);
+                have_play_state_ = true;
+                set_static_obstacles();
             });
         game_settings_sub_ = node->create_subscription<rj_msgs::msg::GameSettings>(
             config_server::topics::kGameSettingsTopic, rclcpp::QoS(1),
@@ -60,11 +63,6 @@
             [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr global_obstacles) {  // NOLINT
                 last_global_obstacles_ = rj_convert::convert_from_ros(*global_obstacles);
             });
-        def_area_obstacles_sub_ = node->create_subscription<rj_geometry_msgs::msg::ShapeSet>(
-            planning::topics::kDefAreaObstaclesTopic, rclcpp::QoS(1),
-            [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr def_area_obstacles) {  // NOLINT
-                last_def_area_obstacles_ = rj_convert::convert_from_ros(*def_area_obstacles);
-            });
         world_state_sub_ = node->create_subscription<rj_msgs::msg::WorldState>(
             vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1),
             [this](rj_msgs::msg::WorldState::SharedPtr world_state) {  // NOLINT
@@ -75,6 +73,13 @@
             [this](rj_msgs::msg::CoachState::SharedPtr coach_state) {  // NOLINT
                 last_coach_state_ = *coach_state;
             });
+        field_dimensions_sub_ = node->create_subscription<rj_msgs::msg::FieldDimensions>(
+            ::config_server::topics::kFieldDimensionsTopic, 10,
+            [this](const rj_msgs::msg::FieldDimensions::SharedPtr msg) {
+                current_field_dimensions_ = rj_convert::convert_from_ros(*msg);
+                have_field_dimensions_ = true;
+                set_static_obstacles();
+            });
     }
 
     [[nodiscard]] PlayState play_state() const {
@@ -107,14 +112,56 @@
     rclcpp::Subscription<rj_geometry_msgs::msg::ShapeSet>::SharedPtr def_area_obstacles_sub_;
     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::FieldDimensions>::SharedPtr field_dimensions_sub_;
 
     PlayState last_play_state_ = PlayState::halt();
+    FieldDimensions current_field_dimensions_;
+    bool have_field_dimensions_;
+    bool have_play_state_;
     GameSettings last_game_settings_;
     int last_goalie_id_;
     rj_geometry::ShapeSet last_global_obstacles_;
     rj_geometry::ShapeSet last_def_area_obstacles_;
     WorldState last_world_state_;
     rj_msgs::msg::CoachState last_coach_state_;
+
+    rj_geometry::ShapeSet create_defense_area_obstacles() {
+        // need field dimensions and to be initialized for this to
+        // work
+        // Create defense areas as rectangular area obstacles
+        auto our_defense_area{std::make_shared<rj_geometry::Rect>(
+            std::move(current_field_dimensions_.our_defense_area()))};
+
+        // Sometimes there is a greater distance we need to keep:
+        // https://robocup-ssl.github.io/ssl-rules/sslrules.html#_robot_too_close_to_opponent_defense_area
+        // TODO(sid-parikh): update this conditional. gameplay_node used different set of checks
+        // than rules imply
+        bool is_extra_dist_necessary = (last_play_state_.state() == PlayState::State::Stop ||
+                                        last_play_state_.restart() == PlayState::Restart::Free);
+
+        // Also add a slack around the box
+        float slack_around_box{0.3f};
+
+        auto their_defense_area =
+            is_extra_dist_necessary
+                ? std::make_shared<rj_geometry::Rect>(std::move(
+                      current_field_dimensions_.their_defense_area_padded(slack_around_box)))
+                : std::make_shared<rj_geometry::Rect>(
+                      std::move(current_field_dimensions_.their_defense_area()));
+
+        // Combine both defense areas into ShapeSet
+        rj_geometry::ShapeSet def_area_obstacles{};
+        def_area_obstacles.add(our_defense_area);
+        def_area_obstacles.add(their_defense_area);
+
+        return def_area_obstacles;
+    }
+
+    void set_static_obstacles() {
+        if (have_field_dimensions_ && have_play_state_) {
+            last_def_area_obstacles_ = create_defense_area_obstacles();
+        }
+    }
 };
 
 /**

soccer/src/soccer/strategy/coach/coach_node.cpp

@@ -8,9 +8,6 @@ CoachNode::CoachNode(const rclcpp::NodeOptions& options) : Node("coach_node", op
         this->create_publisher<rj_msgs::msg::CoachState>(topics::kCoachStateTopic, 10);
     coach_action_callback_timer_ = this->create_wall_timer(100ms, [this]() { coach_ticker(); });
 
-    def_area_obstacles_pub_ = this->create_publisher<rj_geometry_msgs::msg::ShapeSet>(
-        ::planning::topics::kDefAreaObstaclesTopic, 10);
-
     global_obstacles_pub_ = this->create_publisher<rj_geometry_msgs::msg::ShapeSet>(
         ::planning::topics::kGlobalObstaclesTopic, 10);
 
@@ -365,44 +362,12 @@ void CoachNode::goalie_callback(const rj_msgs::msg::Goalie::SharedPtr& msg) {
 
 void CoachNode::publish_static_obstacles() {
     if (have_field_dimensions_) {
-        rj_geometry::ShapeSet defense_area_obstacles = create_defense_area_obstacles();
         rj_geometry::ShapeSet goal_wall_obstacles = create_goal_wall_obstacles();
 
-        def_area_obstacles_pub_->publish(rj_convert::convert_to_ros(defense_area_obstacles));
         global_obstacles_pub_->publish(rj_convert::convert_to_ros(goal_wall_obstacles));
     }
 }
 
-rj_geometry::ShapeSet CoachNode::create_defense_area_obstacles() {
-    // Create defense areas as rectangular area obstacles
-    auto our_defense_area{std::make_shared<rj_geometry::Rect>(
-        std::move(current_field_dimensions_.our_defense_area()))};
-
-    // Sometimes there is a greater distance we need to keep:
-    // https://robocup-ssl.github.io/ssl-rules/sslrules.html#_robot_too_close_to_opponent_defense_area
-    // TODO(sid-parikh): update this conditional. gameplay_node used different set of checks
-    // than rules imply
-    bool is_extra_dist_necessary = (current_play_state_.state == PlayState::State::Stop ||
-                                    current_play_state_.restart == PlayState::Restart::Free);
-
-    // Also add a slack around the box
-    float slack_around_box{0.3f};
-
-    auto their_defense_area =
-        is_extra_dist_necessary
-            ? std::make_shared<rj_geometry::Rect>(
-                  std::move(current_field_dimensions_.their_defense_area_padded(slack_around_box)))
-            : std::make_shared<rj_geometry::Rect>(
-                  std::move(current_field_dimensions_.their_defense_area()));
-
-    // Combine both defense areas into ShapeSet
-    rj_geometry::ShapeSet def_area_obstacles{};
-    def_area_obstacles.add(our_defense_area);
-    def_area_obstacles.add(their_defense_area);
-
-    return def_area_obstacles;
-}
-
 rj_geometry::ShapeSet CoachNode::create_goal_wall_obstacles() {
     // Put all the walls into a ShapeSet and publish it
     rj_geometry::ShapeSet goal_wall_obstacles{};

soccer/src/soccer/strategy/coach/coach_node.hpp

@@ -58,11 +58,6 @@ class CoachNode : public rclcpp::Node {
 private:
     rclcpp::Publisher<rj_msgs::msg::CoachState>::SharedPtr coach_state_pub_;
 
-    /*
-     * Used to publish the defense areas as shapes for the planner node to recognize as obstacles
-     * for non-goalies.
-     */
-    rclcpp::Publisher<rj_geometry_msgs::msg::ShapeSet>::SharedPtr def_area_obstacles_pub_;
     /*
      * Used to add the walls of the goals to the list of global obstacles.
      */
@@ -136,11 +131,6 @@ class CoachNode : public rclcpp::Node {
      */
     void publish_static_obstacles();
 
-    /*
-     * Calculates the defense area as a set of obstacles. The defense area, per the rules, is the
-     * box in front of each goal where only that team's goalie can be in and touch the ball.
-     */
-    rj_geometry::ShapeSet create_defense_area_obstacles();
 
     /*
      * Calculates the physical (3D) walls of the each goal as a set of obstacles, so robots don't