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

@@ -42,15 +42,16 @@
 
 // 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 {
 
-// Client Handles for coordinators
 struct ClientHandles {
     std::unique_ptr<KickerPickerClient> kicker_picker;
+    std::unique_ptr<MarkingClient> marking;
 };
 
 /*
@@ -318,12 +319,12 @@ class Position {
     // protected to allow WorldState to be accessed directly by deriveed
     WorldState* last_world_state_;
 
-    // Current goalie
-    int goalie_id_;
-
     // Client Handles
     std::shared_ptr<ClientHandles> client_handles_;
 
+    // Current goalie
+    int goalie_id_;
+
 private:
     /**
      * @brief allow derived classes to change behavior of get_task(). See

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 {
 
 /*
@@ -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,10 @@ 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/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/coordinator/marking.hpp

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <limits>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <rj_common/field_dimensions.hpp>
+#include <rj_common/world_state.hpp>
+#include <rj_constants/constants.hpp>
+#include <rj_constants/topic_names.hpp>
+#include <rj_convert/ros_convert.hpp>
+#include <rj_msgs/msg/marking.hpp>
+#include <rj_msgs/msg/world_state.hpp>
+#include <rj_msgs/srv/marking.hpp>
+
+#include "rj_strategy/coordinator.hpp"
+
+namespace strategy {
+
+class Marking : public Coordinator<Marking, rj_msgs::srv::Marking, rj_msgs::msg::Marking> {
+public:
+    static constexpr uint8_t kInvalidRobotId = kNumShells;
+
+    Marking();
+    ~Marking() override = default;
+    Marking(const Marking&) = delete;
+    Marking& operator=(const Marking&) = delete;
+    Marking(Marking&&) = delete;
+    Marking& operator=(Marking&&) = delete;
+
+    void service_callback(RequestPtr request, ResponsePtr response);
+
+private:
+    void publish_marking_list();
+    void update_danger_scores();
+    uint8_t find_their_robot_in_possession();
+    uint8_t most_dangerous_robot(uint8_t robotInPossession);
+
+    static constexpr int kMaxMarkers = 2;
+    // this is the threshold value for switching
+    // you can play with these constants if the current results aren't good enough
+    static constexpr double kSuperDangerSub = 3.2;
+    static constexpr double kDangerDistToBall = 3.0;
+    static constexpr double kDangerDistToGoal = 5.0;
+    static constexpr double kDangerDistToOurRobots = 3.0;
+    static constexpr double kDangerAngle = 2.0;
+    static constexpr double kPossessionThreshold = 0.3;
+    int num_markers_;
+
+    std::array<uint8_t, kNumShells> marking_list_{};
+    std::array<double, kNumShells> danger_score_{};
+    std::array<uint8_t, kNumShells> enemy_to_friends_{};
+    std::unordered_set<uint8_t> unassigned_markers_queue_;
+    WorldState last_world_state_;
+    FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions;
+    rclcpp::Subscription<rj_msgs::msg::WorldState>::SharedPtr world_state_sub_;
+};
+
+}  // namespace strategy

src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+#include <spdlog/spdlog.h>
+
+#include <rj_msgs/msg/marking.hpp>
+#include <rj_msgs/srv/marking.hpp>
+
+#include "rj_constants/constants.hpp"
+
+namespace strategy {
+
+/**
+ * @brief Client for interacting with the KickerPicker coordinator.
+ *
+ * Manages membership in the kicker group and tracks the currently selected kicker.
+ */
+class MarkingClient {
+public:
+    static constexpr uint8_t kInvalidRobotId = kNumShells;
+
+    using StatusCallback = std::function<void(bool)>;
+
+    explicit MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id);
+    ~MarkingClient() = default;
+    MarkingClient(const MarkingClient&) = delete;
+    MarkingClient& operator=(const MarkingClient&) = delete;
+    MarkingClient(MarkingClient&&) = delete;
+    MarkingClient& operator=(MarkingClient&&) = delete;
+
+    /**
+     * @brief Join the marking group.
+     * @param callback Called with current membership status after attempt to join.
+     */
+    void join_group(StatusCallback callback = nullptr);
+
+    /**
+     * @brief Leave the marking group.
+     * @param callback Called with current membership status after attempt to leave.
+     */
+    void leave_group(StatusCallback callback = nullptr);
+
+    /**
+     * @brief Check if this robot is a member of the marker group.
+     */
+    [[nodiscard]] bool am_i_member() const;
+
+    /**
+     * @brief Get the currently selected enemey robot id marking.
+     * @return robot ID of selected robot, or kInvalidRobotId if none selected.
+     */
+    [[nodiscard]] uint8_t who_am_i_marking() const;
+
+    /**
+     * @brief Check if this robot is currently marking.
+     */
+    [[nodiscard]] bool am_i_marking() const;
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    const uint8_t robot_id_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) -- class
+                              // isn't move/copy-able anyway
+    rclcpp::Client<rj_msgs::srv::Marking>::SharedPtr client_;
+    rclcpp::Subscription<rj_msgs::msg::Marking>::SharedPtr subscription_;
+    bool am_i_member_{false};
+    bool am_i_marking_{false};
+    uint8_t selected_robot_marking_id_{kInvalidRobotId};
+};
+
+}  // namespace strategy