RobotFactoryPosition Layer

launch/soccer.launch.py

@@ -180,7 +180,8 @@ def generate_launch_description():
                 output="screen",
                 on_exit=Shutdown(),
             ),
-            # spawn strategy only if manual is not on
+            # spawn internal_ref/external_ref based on internal_ref
+            # LaunchArgument
             Node(
                 condition=IfCondition(PythonExpression(["not ", use_manual_control])),
                 package="rj_robocup",
@@ -189,15 +190,6 @@ def generate_launch_description():
                 parameters=[param_config_filepath],
                 on_exit=Shutdown(),
             ),
-            Node(
-                condition=IfCondition(PythonExpression(["not ", use_manual_control])),
-                package="rj_robocup",
-                executable="coach_node",
-                output="screen",
-                parameters=[param_config_filepath],
-                on_exit=Shutdown(),
-            ),
-            # spawn internal_ref/external_ref based on internal_ref LaunchArgument
             Node(
                 condition=IfCondition(PythonExpression([use_internal_ref])),
                 package="rj_robocup",

rj_common/include/rj_common/field_dimensions.hpp

@@ -103,7 +103,7 @@ struct FieldDimensions {
 
     /*
      * Provides a rect that is a padded version of their goalbox.
-     * Used as a static obstalce by coach in certain match situations.
+     * Used as a static obstacle in certain match situations.
      */
     [[nodiscard]] rj_geometry::Rect their_defense_area_padded(float padding) {
         rj_geometry::Rect tmp = rj_geometry::Rect(their_defense_area_);

rj_common/include/rj_common/network.hpp

@@ -59,7 +59,7 @@ static const std::string kSharedVisionSourceAddress = "224.5.23.2";
  * one (or try them all in worst-case).
  */
 
-static const std::string kRefereeInterface = "192.168.20.119";
+static const std::string kRefereeInterface = "127.0.0.1";
 /* static const std::string kRefereeInterface = "172.0.0.1"; */
 static const std::string kVisionInterface =
     kRefereeInterface;  // In all but rare cirucmstances, this should match kRefereeInterface.

rj_config/src/config_server/config_server.cpp

@@ -54,8 +54,10 @@ void ConfigServer::broadcast_field_dimensions() {
 }
 
 void ConfigServer::set_field_dimensions_callback(const FieldDimensionsMsg& msg) {
-    field_dimensions_ = msg;
-    broadcast_field_dimensions();
+    if (field_dimensions_ != msg) {
+        field_dimensions_ = msg;
+        broadcast_field_dimensions();
+    }
 }
 
 }  // namespace config_server

rj_constants/include/rj_constants/topic_names.hpp

@@ -69,8 +69,6 @@ static inline std::string trajectory_topic(int robot_id) {
 }  // namespace planning::topics
 
 namespace strategy::topics {
-constexpr auto kCoachStateTopic{"/strategy/coach_state"};
-constexpr auto kPositionsTopic{"/strategy/positions"};
 }  // namespace strategy::topics
 
 namespace control {
@@ -108,4 +106,4 @@ static inline std::string robot_status_topic(int robot_id) {
     return "radio/robot_status/robot_" + std::to_string(robot_id);
 }
 
-}  // namespace radio::topics
+}  // namespace radio::topics

rj_msgs/CMakeLists.txt

@@ -27,14 +27,12 @@ rosidl_generate_interfaces(
   msg/AliveRobots.msg
   msg/BallState.msg
   msg/BallPlacement.msg
-  msg/CoachState.msg
   msg/DetectionBall.msg
   msg/DetectionFrame.msg
   msg/DetectionRobot.msg
   msg/FieldDimensions.msg
   msg/FieldOrientation.msg
   msg/GameSettings.msg
-  msg/GlobalOverride.msg
   msg/Goalie.msg
   msg/ManipulatorSetpoint.msg
   msg/MatchState.msg
@@ -54,7 +52,6 @@ rosidl_generate_interfaces(
   msg/TeamInfo.msg
   msg/Trajectory.msg
   msg/WorldState.msg
-  msg/PositionAssignment.msg
 
   # Communication
   msg/AgentRequest.msg

rj_protos/protos/Control.proto

@@ -13,7 +13,7 @@ message Control {
         ON_BREAK_BEAM = 2;
     }
 
-    // LEDs on the robot display different colors based on current role from Coach
+    // LEDs on the robot display different colors based on current role
     enum Role {
         GOALIE = 0;
         DEFENSE = 1;

soccer/CMakeLists.txt

@@ -85,7 +85,6 @@ add_executable(sim_radio_node)
 add_executable(network_radio_node)
 add_executable(manual_control_node)
 add_executable(global_param_server_node)
-add_executable(coach_node)
 add_executable(agent_action_client_node)
 
 if(BUILD_TESTING)
@@ -170,9 +169,6 @@ set(LOG_VIEWER_DEPS_LIBRARIES robocup)
 set(REFEREE_NODE_DEPS_SYSTEM_LIBRARIES)
 set(REFEREE_NODE_DEPS_LIBRARIES robocup)
 
-set(COACH_NODE_DEPS_SYSTEM_LIBRARIES)
-set(COACH_NODE_DEPS_LIBRARIES robocup)
-
 set(RADIO_NODE_DEPS_SYSTEM_LIBRARIES)
 set(RADIO_NODE_DEPS_LIBRARIES robocup)
 
@@ -229,9 +225,6 @@ target_link_libraries(internal_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_L
 target_link_libraries(sim_radio_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES}
         ${RADIO_NODE_DEPS_LIBRARIES})
 
-target_link_libraries(coach_node PRIVATE ${COACH_NODE_DEPS_SYSTEM_LIBRARIES}
-        ${COACH_NODE_DEPS_LIBRARIES})
-
 # -- network_radio_node --
 target_link_libraries(network_radio_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES}
         ${RADIO_NODE_DEPS_LIBRARIES})
@@ -296,7 +289,6 @@ install(
         planner_node
         control_node
         sim_radio_node
-        coach_node
         network_radio_node
         manual_control_node
         global_param_server_node

soccer/src/soccer/CMakeLists.txt

@@ -52,7 +52,6 @@ set(ROBOCUP_LIB_SRC
     radio/network_radio.cpp
     radio/packet_convert.cpp
     radio/sim_radio.cpp
-    strategy/coach/coach_node.cpp
     radio/radio.cpp
     referee/referee_base.cpp
     referee/external_referee.cpp
@@ -72,12 +71,12 @@ set(ROBOCUP_LIB_SRC
     ros2_temp/soccer_config_client.cpp
     ros2_temp/raw_vision_packet_sub.cpp
     ros2_temp/referee_sub.cpp
-    ros2_temp/coach_sub.cpp
     ros2_temp/autonomy_interface.cpp
     ros2_temp/debug_draw_interface.cpp
     strategy/agent/agent_action_client.cpp
     strategy/agent/communication/communication.cpp
     strategy/agent/position/position.cpp
+    strategy/agent/position/robot_factory_position.cpp
     strategy/agent/position/goalie.cpp
     strategy/agent/position/offense.cpp
     strategy/agent/position/defense.cpp
@@ -122,8 +121,6 @@ set(CONTROL_NODE_SRC control/control_node_main.cpp)
 
 set(SIM_RADIO_NODE_SRC radio/sim_radio_node_main.cpp)
 
-set(COACH_NODE_SRC strategy/coach/coach_node_main.cpp)
-
 set(NETWORK_RADIO_NODE_SRC radio/network_radio_node_main.cpp)
 
 set(MANUAL_CONTROL_NODE_SRC joystick/manual_control_node_main.cpp)
@@ -163,9 +160,6 @@ target_sources(control_node PRIVATE ${CONTROL_NODE_SRC})
 # ---- sim_radio_node ----
 target_sources(sim_radio_node PRIVATE ${SIM_RADIO_NODE_SRC})
 
-# ---- coach_node ----
-target_sources(coach_node PRIVATE ${COACH_NODE_SRC})
-
 # ---- sim_radio_node ----
 target_sources(network_radio_node PRIVATE ${NETWORK_RADIO_NODE_SRC})
 

soccer/src/soccer/context.hpp

@@ -36,6 +36,7 @@ struct Context {
     // Radio -> Gameplay
     std::array<RobotStatus, kNumShells> robot_status;
     // Coach -> Positions
+    // TODO(sid-parikh) Delete Robot_Positions UI stuff
     std::array<uint32_t, kNumShells> robot_positions;
     // MainWindow -> Manual control
     std::array<bool, kNumShells> is_joystick_controlled{};

soccer/src/soccer/control/motion_control.cpp

@@ -7,6 +7,7 @@
 #include <rj_geometry/util.hpp>
 #include <rj_utils/logging.hpp>
 
+#include "game_state.hpp"
 #include "planning/instant.hpp"
 
 namespace control {

soccer/src/soccer/control/motion_control.hpp

@@ -7,6 +7,7 @@
 #include <rj_param_utils/param.hpp>
 
 #include "control/motion_setpoint.hpp"
+#include "game_state.hpp"
 #include "ros_debug_drawer.hpp"
 
 #include <rc-fshare/pid.hpp>
@@ -96,4 +97,4 @@ class MotionControl {
     rclcpp::Publisher<RobotState::Msg>::SharedPtr target_state_pub_;
 };
 
-}  // namespace control
+}  // namespace control

soccer/src/soccer/control/motion_control_test.cpp

@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
 
+#include "game_state.hpp"
 #include "motion_control_node.hpp"
 
 namespace control::testing {
@@ -129,4 +130,4 @@ TEST_F(MotionControlTest, coordinate_transform_body_x) {
     EXPECT_NEAR(setpoint.avelocity, 0.0, 1e-6);
 }
 
-}  // namespace control::testing
+}  // namespace control::testing

soccer/src/soccer/planning/global_state.cpp

@@ -6,8 +6,12 @@ GlobalState::GlobalState(rclcpp::Node* node) {
     play_state_sub_ = node->create_subscription<rj_msgs::msg::PlayState>(
         referee::topics::kPlayStateTopic, rclcpp::QoS(1),
         [this](rj_msgs::msg::PlayState::SharedPtr state) {  // NOLINT
-            auto lock = std::lock_guard(last_play_state_mutex_);
-            last_play_state_ = rj_convert::convert_from_ros(*state);
+            {
+                auto lock = std::lock_guard(last_play_state_mutex_);
+                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),
@@ -27,30 +31,29 @@ GlobalState::GlobalState(rclcpp::Node* node) {
             auto lock = std::lock_guard(last_global_obstacles_mutex_);
             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
-            auto lock = std::lock_guard(last_def_area_obstacles_mutex_);
-            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
             auto lock = std::lock_guard(last_world_state_mutex_);
             last_world_state_ = rj_convert::convert_from_ros(*world_state);
         });
-    coach_state_sub_ = node->create_subscription<rj_msgs::msg::CoachState>(
-        "/strategy/coach_state", rclcpp::QoS(1),
-        [this](rj_msgs::msg::CoachState::SharedPtr coach_state) {  // NOLINT
-            auto lock = std::lock_guard(last_coach_state_mutex_);
-            last_coach_state_ = *coach_state;
+    field_dimensions_sub_ = node->create_subscription<rj_msgs::msg::FieldDimensions>(
+        ::config_server::topics::kFieldDimensionsTopic, rclcpp::QoS(1).transient_local(),
+        [this](const rj_msgs::msg::FieldDimensions::SharedPtr msg) {  // NOLINT
+            {
+                auto lock = std::lock_guard(last_field_dimensions_mutex_);
+                last_field_dimensions_ = rj_convert::convert_from_ros(*msg);
+                have_field_dimensions_ = true;
+            }
+            set_static_obstacles();
         });
 }
 
 [[nodiscard]] PlayState GlobalState::play_state() const {
     auto lock = std::lock_guard(last_play_state_mutex_);
     return last_play_state_;
 }
+
 [[nodiscard]] GameSettings GlobalState::game_settings() const {
     auto lock = std::lock_guard(last_game_settings_mutex_);
     return last_game_settings_;
@@ -71,9 +74,44 @@ GlobalState::GlobalState(rclcpp::Node* node) {
     auto lock = std::lock_guard(last_world_state_mutex_);
     return &last_world_state_;
 }
-[[nodiscard]] const rj_msgs::msg::CoachState GlobalState::coach_state() const {
-    auto lock = std::lock_guard(last_coach_state_mutex_);
-    return last_coach_state_;
+
+rj_geometry::ShapeSet GlobalState::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>(last_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
+    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>(
+                  last_field_dimensions_.their_defense_area_padded(slack_around_box))
+            : std::make_shared<rj_geometry::Rect>(last_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 GlobalState::set_static_obstacles() {
+    std::scoped_lock lock{last_field_dimensions_mutex_, last_play_state_mutex_,
+                          last_def_area_obstacles_mutex_};
+
+    if (have_field_dimensions_ && have_play_state_) {
+        // SPDLOG_INFO("Creating new defense obstacles");
+        last_def_area_obstacles_ = create_defense_area_obstacles();
+    }
 }
 
 }  // namespace planning