Waller 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="waller_node",
+                output="screen",
+                parameters=[param_config_filepath],
+                on_exit=Shutdown(),
             )
         ]
     )

src/rj_msgs/CMakeLists.txt

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

src/rj_msgs/msg/Waller.msg

@@ -0,0 +1,3 @@
+# This is a message for the waller coordinator
+uint8[16] wall_list  # Sorted IDs of robots on the wall
+uint8 wall_size      # Number of robots on the wall

src/rj_msgs/srv/Waller.srv

@@ -0,0 +1,5 @@
+# Request a robot to be added to or removed from the waller selection group
+uint8 robot_id     # ID of the robot (0 to kNumShells-1)
+bool joining       # True to add robot to waller group, false to remove it
+---
+bool success      # True if the ACTION succeded (True for joining means successfully joined, for leaving means successfully left)

src/rj_strategy/CMakeLists.txt

@@ -56,14 +56,14 @@ set(RJ_STRATEGY_SRC
   src/agent/position/seeker.cpp
   src/agent/position/smartidling.cpp
   src/agent/position/solo_offense.cpp
-  src/agent/position/waller.cpp
   src/agent/position/zoner.cpp
 
   # Communication
   src/agent/communication.cpp
 
   # Coordinators
   src/coordinator/kicker_picker_client.cpp
+  src/coordinator/waller_client.cpp
 )
 
 # rj_strategy library
@@ -115,6 +115,23 @@ ament_target_dependencies(kicker_picker_node
   ${RJ_STRATEGY_DEPS}
 )
 
+# Waller Node
+add_executable(waller_node
+  ${RJ_STRATEGY_SRC}
+  src/coordinator/waller_client.cpp
+  src/coordinator/waller.cpp
+)
+
+target_include_directories(waller_node
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(waller_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 waller_node agent_action_clients
   DESTINATION lib/${PROJECT_NAME}
 )
 

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

@@ -42,6 +42,7 @@
 
 // Coordinators
 #include "rj_strategy/coordinator/kicker_picker_client.hpp"
+#include "rj_strategy/coordinator/waller_client.hpp"
 
 // tell compiler this class exists, but no need to import the whole header
 class AgentActionClient;
@@ -51,6 +52,7 @@ namespace strategy {
 // Client Handles for coordinators
 struct ClientHandles {
     std::unique_ptr<KickerPickerClient> kicker_picker;
+    std::unique_ptr<WallerClient> waller;
 };
 
 /*

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

@@ -17,7 +17,6 @@
 
 #include "rj_strategy/agent/position.hpp"
 #include "rj_strategy/agent/position/marker.hpp"
-#include "rj_strategy/agent/position/waller.hpp"
 
 namespace strategy {
 
@@ -44,10 +43,6 @@ 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
-
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
      *  to do based on the game situation. The method will continuously look to assign
@@ -72,54 +67,9 @@ class Defense : public Position {
     };
 
     State update_state();
-
+    State current_state_ = IDLING;
     std::optional<RobotIntent> state_to_task(RobotIntent intent);
 
-    /**
-     * @brief Sends a JoinWallRequest in broadcast to the other robots
-     */
-    void send_join_wall_request();
-
-    /**
-     * @brief Sends a LeaveWallRequest to each of the robots in walling_robots_.
-     */
-    void send_leave_wall_request();
-
-    /**
-     * @brief Adds the new waller to this robot's list of wallers and updates this robot's position
-     * in the wall.
-     *
-     * @param join_request the request received from another robot about joining the wall
-     * @return communication::JoinWallResponse A confirmation for the other robot to join the wall
-     * with this robot's ID
-     */
-    communication::JoinWallResponse handle_join_wall_request(
-        communication::JoinWallRequest join_request);
-
-    /**
-     * @brief Removes a given robot from this robot's list of wallers.
-     *
-     * @param leave_request the request from the robot who is leaving the wall
-     * @return communication::Acknowledge acknowledgement of the other robot's communication
-     */
-    communication::Acknowledge handle_leave_wall_request(
-        communication::LeaveWallRequest leave_request);
-
-    /**
-     * @brief Handles the response from the currently walling robots to find this robot's place in
-     * the wall.
-     *
-     * @param join_response the response from another robot that this robot can join the wall
-     */
-    void handle_join_wall_response(communication::JoinWallResponse join_response);
-
-    std::vector<u_int8_t> walling_robots_ = {};
-    int waller_id_ = -1;
-
-    // current state of the defense agent (state machine)
-    int get_waller_id();
-    State current_state_ = JOINING_WALL;
-
     int get_marker_target_id();
     Marker marker_;
 };

src/rj_strategy/include/rj_strategy/coordinator/waller.hpp

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <algorithm>
+#include <array>
+
+#include <rclcpp/rclcpp.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/waller.hpp>
+#include <rj_msgs/msg/world_state.hpp>
+#include <rj_msgs/srv/waller.hpp>
+
+#include "rj_strategy/coordinator.hpp"
+
+namespace strategy {
+
+class Waller : public Coordinator<Waller, rj_msgs::srv::Waller, rj_msgs::msg::Waller> {
+public:
+    Waller();
+    ~Waller() override = default;
+    Waller(const Waller&) = delete;
+    Waller& operator=(const Waller&) = delete;
+    Waller(Waller&&) = delete;
+    Waller& operator=(Waller&&) = delete;
+
+    void service_callback(RequestPtr request, ResponsePtr response);
+
+private:
+    std::array<uint8_t, kNumShells> walling_robots_;
+    uint8_t num_wallers_ = 0;
+    static constexpr int kMaxWallers = 4;
+
+    void update_wallers();
+};
+
+}  // namespace strategy

src/rj_strategy/include/rj_strategy/coordinator/waller_client.hpp

@@ -0,0 +1,92 @@
+#pragma once
+
+#include <cmath>
+
+#include <rclcpp/rclcpp.hpp>
+#include <spdlog/spdlog.h>
+
+#include <rj_common/field_dimensions.hpp>
+#include <rj_msgs/msg/waller.hpp>
+#include <rj_msgs/srv/waller.hpp>
+
+#include "rj_strategy/coordinator/waller.hpp"
+
+namespace strategy {
+
+/**
+ * @brief Client for interacting with the Waller coordinator.
+ *
+ * Manages membership in the waller group and tracks the currently waller list.
+ */
+class WallerClient {
+public:
+    struct Result {
+        bool success{false};
+    };
+
+    using StatusCallback = std::function<void(Result)>;
+
+    explicit WallerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id);
+    ~WallerClient() = default;
+    WallerClient(const WallerClient&) = delete;
+    WallerClient& operator=(const WallerClient&) = delete;
+    WallerClient(WallerClient&&) = delete;
+    WallerClient& operator=(WallerClient&&) = delete;
+
+    /**
+     * @brief Join the waller group.
+     * @param callback Called with current membership status after attempt to join.
+     */
+    void join_group(StatusCallback callback = nullptr);
+
+    /**
+     * @brief Leave the waller 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 waller group.
+     */
+    [[nodiscard]] bool am_i_member() const;
+
+    /**
+     * @brief Get the target walling point
+     * @return target walling point of this robot.
+     */
+    [[nodiscard]] std::optional<rj_geometry::Point> get_walling_point(
+        const WorldState* world_state, FieldDimensions field_dimensions) 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::Waller>::SharedPtr client_;
+    rclcpp::Subscription<rj_msgs::msg::Waller>::SharedPtr subscription_;
+
+    bool am_i_member_ = false;
+    bool request_pending_ = false;
+    std::array<uint8_t, kNumShells> walling_robots_;
+    int num_wallers_ = 0;
+
+    static constexpr double kRobotDiameterMultiplier = 1.5;
+
+    struct WallerGeometry {
+        double min_wall_radius;
+        double wall_spacing;
+        rj_geometry::Point goal_pos;
+        rj_geometry::Point ball_pos;
+        rj_geometry::Point robot_pos;
+    };
+
+    WallerGeometry calculate_wall_geometry(const WorldState* world_state,
+                                           FieldDimensions dimensions) const;
+    rj_geometry::Point get_target_position(WallerGeometry& waller_geometry, long waller_pos) const;
+    std::optional<uint8_t> get_parent_id(WallerGeometry& waller_geometry,
+                                         rj_geometry::Point target_point, long waller_pos) const;
+    rj_geometry::Point get_target_position_with_parent(WallerGeometry& waller_geometry,
+                                                       rj_geometry::Point target_point,
+                                                       rj_geometry::Point parent_point) const;
+};
+
+}  // namespace strategy