Marking coordinator

install/setup.bash

@@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null
 _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
 
 unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_bash_source_script
+unset _colcon_prefix_chain_bash_source_script

install/setup.zsh

@@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && p
 _colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
 
 unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_zsh_source_script
+unset _colcon_prefix_chain_zsh_source_script

launch/soccer.launch.py

@@ -237,6 +237,13 @@ def generate_launch_description():
                 output="screen",
                 parameters=[param_config_filepath],
                 on_exit=Shutdown(),
+            ),
+            Node(
+                package="rj_strategy",
+                executable="marking_node",
+                output="screen",
+                parameters=[param_config_filepath],
+                on_exit=Shutdown(),
             )
         ]
     )

src/rj_constants/include/rj_constants/constants.hpp

@@ -7,6 +7,7 @@
 /// all weights in kilograms
 
 // Number of identifiable robots on one team
+// IF U CHANGE THIS NUMBER CHECK Marking.msg
 inline constexpr size_t kNumShells = 16;
 
 // Number of playing robots on one team

src/rj_msgs/CMakeLists.txt

@@ -59,6 +59,7 @@ rosidl_generate_interfaces(
 
   # Coordinators
   msg/KickerPicker.msg
+  msg/Marking.msg
 
   # Agent Request Messages
   request/JoinWallRequest.msg
@@ -84,6 +85,7 @@ rosidl_generate_interfaces(
   # Services
   srv/AgentCommunication.srv
   srv/KickerPicker.srv
+  srv/Marking.srv
   srv/ListJoysticks.srv
   srv/PlanHypotheticalPath.srv
   srv/QuickCommands.srv

src/rj_msgs/msg/Marking.msg

@@ -0,0 +1,2 @@
+# This is a message to pick a robot to mark
+uint8[16] mark_robot_ids  # ID of the robot to mark on the opposing team

src/rj_msgs/srv/Marking.srv

@@ -0,0 +1,5 @@
+# Request a robot to be added to or removed from the marking group
+uint8 robot_id     # ID of the robot (0 to kNumShells-1)
+bool join
+---
+bool success      # Always true

src/rj_strategy/CMakeLists.txt

@@ -47,7 +47,6 @@ set(RJ_STRATEGY_SRC
   src/agent/position/goalie.cpp
   src/agent/position/idle.cpp
   src/agent/position/line.cpp
-  src/agent/position/marker.cpp
   src/agent/position/offense.cpp
   src/agent/position/penalty_non_kicker.cpp
   src/agent/position/penalty_player.cpp
@@ -64,6 +63,7 @@ set(RJ_STRATEGY_SRC
 
   # Coordinators
   src/coordinator/kicker_picker_client.cpp
+  src/coordinator/marking_client.cpp
 )
 
 # rj_strategy library
@@ -115,6 +115,23 @@ ament_target_dependencies(kicker_picker_node
   ${RJ_STRATEGY_DEPS}
 )
 
+# Marking Node
+add_executable(marking_node
+  ${RJ_STRATEGY_SRC}
+  src/coordinator/marking_client.cpp
+  src/coordinator/marking.cpp
+)
+
+target_include_directories(marking_node
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(marking_node
+  ${RJ_STRATEGY_DEPS}
+)
+
 # Agent Action Clients
 add_executable(agent_action_clients
   ${RJ_STRATEGY_SRC}
@@ -146,7 +163,7 @@ install(
 )
 
 install(
-  TARGETS straight_line_test_node kicker_picker_node agent_action_clients
+  TARGETS straight_line_test_node kicker_picker_node marking_node agent_action_clients
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/rj_strategy/include/rj_strategy/agent/position.hpp

@@ -40,11 +40,20 @@
 #include <rj_msgs/msg/position_request.hpp>
 #include <rj_msgs/msg/test_response.hpp>
 
+// Coordinators
+#include "rj_strategy/coordinator/kicker_picker_client.hpp"
+#include "rj_strategy/coordinator/marking_client.hpp"
+
 // tell compiler this class exists, but no need to import the whole header
 class AgentActionClient;
 
 namespace strategy {
 
+struct ClientHandles {
+    std::unique_ptr<KickerPickerClient> kickerPickerClient;
+    std::unique_ptr<MarkingClient> markingClient;
+};
+
 /*
  * Position is an abstract superclass. Its subclasses handle strategy logic.
  * The goal is to isolate the strategy logic from the ROS interfacing. This
@@ -60,7 +69,7 @@ class Position {
 public:
     Position(int r_id);
     virtual ~Position() = default;
-    Position(const Position& other) = default;
+    Position(Position&& other) = default;
 
     /**
      * @brief return a RobotIntent to be sent to PlannerNode by AC; nullopt
@@ -229,6 +238,9 @@ class Position {
      */
     virtual void set_goalie_id(int goalie_id);
 
+    // Allow external code (e.g., RobotFactoryPosition) to inject shared client handles
+    void set_client_handles(std::shared_ptr<ClientHandles> client_handles);
+
 protected:
     Position(int r_id, std::string position_name);
 
@@ -305,6 +317,9 @@ class Position {
     // protected to allow WorldState to be accessed directly by deriveed
     WorldState* last_world_state_;
 
+    // Client Handles
+    std::shared_ptr<ClientHandles> client_handles_;
+
     // Current goalie
     int goalie_id_;
 

src/rj_strategy/include/rj_strategy/agent/position/defense.hpp

@@ -16,9 +16,7 @@
 #include <rj_msgs/action/robot_move.hpp>
 
 #include "rj_strategy/agent/position.hpp"
-#include "rj_strategy/agent/position/marker.hpp"
 #include "rj_strategy/agent/position/waller.hpp"
-
 namespace strategy {
 
 /*
@@ -29,7 +27,7 @@ class Defense : public Position {
 public:
     Defense(int r_id);
     ~Defense() override = default;
-    Defense(const Position& other);
+    Defense(Position&& other);
 
     void receive_communication_response(communication::AgentPosResponseWrapper response) override;
     communication::PosAgentResponseWrapper receive_communication_request(
@@ -44,9 +42,8 @@ class Defense : public Position {
     void revive() override;
 
 private:
-    // static constexpr int kMaxWallers{6};
-    static constexpr int kMaxWallers{
-        static_cast<int>(kNumShells)};  // This effectively turns off marking
+    static constexpr int kMaxWallers{2};
+    static constexpr RJ::Seconds kMarkingGroupJoinTimeout{2.0};
 
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
@@ -120,8 +117,11 @@ class Defense : public Position {
     int get_waller_id();
     State current_state_ = JOINING_WALL;
 
-    int get_marker_target_id();
-    Marker marker_;
+    bool sent_join_marking_group_request_ = false;
+    RJ::Time request_time_;
+
+
+    bool pending_marking_state_ = false;
 };
 
 }  // namespace strategy

src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp

@@ -18,7 +18,7 @@ namespace strategy {
 class FreeKicker : public Position {
 public:
     FreeKicker(int r_id);
-    FreeKicker(const Position&);
+    FreeKicker(Position&&);
     ~FreeKicker() = default;
 
     /**

src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp

@@ -41,4 +41,4 @@ class GoalKicker : public Position {
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
 };
 
-}  // namespace strategy
+}  // namespace strategy

src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp

@@ -24,7 +24,7 @@ class Goalie : public Position {
 public:
     Goalie(int r_id);
     ~Goalie() override = default;
-    Goalie(const Position& other);
+    Goalie(Position&& other);
 
     void derived_acknowledge_pass() override;
     void derived_pass_ball() override;

src/rj_strategy/include/rj_strategy/agent/position/idle.hpp

@@ -9,7 +9,7 @@ class Idle : public Position {
 public:
     Idle(int r_id);
     ~Idle() = default;
-    Idle(const Position& other);
+    Idle(Position&& other);
 
     /**
      * @brief Does nothing; this position is a special case

src/rj_strategy/include/rj_strategy/agent/position/line.hpp

@@ -8,7 +8,7 @@
 namespace strategy {
 class Line : public Position {
 public:
-    Line(const Position& other);
+    Line(Position&& other);
     Line(int r_id);
     Line(int r_id, bool forward);
     ~Line() override = default;

src/rj_strategy/include/rj_strategy/agent/position/offense.hpp

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

src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp

@@ -19,7 +19,7 @@ class PenaltyNonKicker : public Position {
 public:
     PenaltyNonKicker(int r_id);
     ~PenaltyNonKicker() = default;
-    PenaltyNonKicker(const Position& other);
+    PenaltyNonKicker(Position&& other);
 
     /**
      * @brief Does nothing; this position is a special case

src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp

@@ -22,7 +22,7 @@ class PenaltyPlayer : public Position {
 public:
     PenaltyPlayer(int r_id);
     ~PenaltyPlayer() = default;
-    PenaltyPlayer(const Position& other);
+    PenaltyPlayer(Position&& other);
 
     /**
      * @brief Does nothing; this position is a special case

src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp

@@ -116,7 +116,6 @@ class RobotFactoryPosition : public Position {
 private:
     std::unique_ptr<Position> current_position_;
 
-    KickerPickerClient kicker_picker_;
     OverridingPositions override_play_position_{OverridingPositions::AUTO};
 
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
@@ -145,10 +144,9 @@ class RobotFactoryPosition : public Position {
     template <class Pos>
     void set_current_position() {
         if (dynamic_cast<Pos*>(current_position_.get()) == nullptr) {
-            // This line requires Pos to implement the constructor Pos(const
-            // Position&)
+            // This line requires Pos to implement the constructor Pos(Position&&)
             current_position_->die();
-            current_position_ = std::make_unique<Pos>(*current_position_);
+            current_position_ = std::make_unique<Pos>(std::move(*current_position_));
             SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name());
         }
     }

src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp

@@ -9,7 +9,7 @@ class SmartIdle : public Position {
 public:
     SmartIdle(int r_id);
     ~SmartIdle() = default;
-    SmartIdle(const Position& other);
+    SmartIdle(Position&& other);
 
     /**
      * @brief Does nothing; this position is a special case

src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp

@@ -22,7 +22,7 @@ namespace strategy {
 
 class SoloOffense : public Position {
 public:
-    SoloOffense(const Position& other);
+    SoloOffense(Position&& other);
     SoloOffense(int r_id);
     ~SoloOffense() override = default;
     SoloOffense(const SoloOffense& other) = default;

src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp

@@ -28,7 +28,7 @@ class Zoner : public Position {
 public:
     Zoner(int r_id);
     ~Zoner() override = default;
-    Zoner(const Position& other);
+    Zoner(Position&& other);
     Zoner(Zoner&& other) = default;
     Zoner& operator=(const Zoner& other) = default;
     Zoner& operator=(Zoner&& other) = default;