Passing improvements (#2516)

* Almost completed behaviour

the passing works, but there are some issues still left in terms of wait time, kick distance, and occasional bugs that need to be worked on.

* Current working implementation of offense passing along with fix (maybe) on grey field bug.

* Reverting some changes to make seperate PRs for them.

* can_i_shoot function implemented. Should now only pass if there's a bot capable on shooting.

* More passing additions. Prevents passing to unessesary recievers or over goalie boxes.

* More changes towards the final PR, needs more work on conditions and add a skip feature if the kick doesn't trigger.

* Failsafe behind passing. If failure to pass, both bots go to default.

* Final change focusing on sending passing requests constantly and defaulting to shooting if no one's open.

* Fix Code Style On passing-improvements (#2517)

automated style fixes

Co-authored-by: CameronLyon <CameronLyon@users.noreply.github.com>

* Final adjustments based on PR comments.

* Fix Code Style On passing-improvements (#2521)

automated style fixes

Co-authored-by: CameronLyon <CameronLyon@users.noreply.github.com>

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: CameronLyon <CameronLyon@users.noreply.github.com>

Author: CameronLyon

src/rj_config_server/src/config_server.cpp

@@ -72,6 +72,7 @@ using config_server::GameSettingsMsg;
  */
 GameSettingsMsg parse_game_settings(const std::vector<std::string>& args) {
     GameSettingsMsg game_settings;
+
     for (size_t i = 1; i < args.size(); i++) {
         const std::string& arg = args.at(i);
         if (arg == "-b") {

src/rj_msgs/CMakeLists.txt

@@ -75,6 +75,7 @@ rosidl_generate_interfaces(
   request/LeaveWallRequest.msg
   request/ResetScorerRequest.msg
   request/IncomingBallRequest.msg
+  request/PassReceivedRequest.msg
 
   # Agent Response Messages
   response/ScorerResponse.msg

src/rj_msgs/msg/AgentRequest.msg

@@ -12,4 +12,5 @@ SeekerRequest[<=1] seeker_request
 PositionRequest[<=1] position_request
 LeaveWallRequest[<=1] leave_wall_request
 ResetScorerRequest[<=1] reset_scorer_request
-IncomingBallRequest[<=1] incoming_ball_request
+IncomingBallRequest[<=1] incoming_ball_request
+PassReceivedRequest[<=1] pass_received_request

src/rj_msgs/request/PassReceivedRequest.msg

@@ -0,0 +1,5 @@
+# Request sent by the receiver to the passer indicating that the receiver
+# has controlled the ball. The passer uses this to transition out of
+# PASSING_FINISHED and back to DEFAULT (so it does not chase the ball).
+uint32 request_uid
+uint8 from_robot_id

src/rj_strategy/include/rj_strategy/agent/communication.hpp

@@ -18,6 +18,7 @@
 #include "rj_strategy/agent/communication/join_wall_response.hpp"
 #include "rj_strategy/agent/communication/leave_wall_request.hpp"
 #include "rj_strategy/agent/communication/leave_wall_response.hpp"
+#include "rj_strategy/agent/communication/pass_received_request.hpp"
 #include "rj_strategy/agent/communication/pass_request.hpp"
 #include "rj_strategy/agent/communication/pass_response.hpp"
 #include "rj_strategy/agent/communication/position_request.hpp"
@@ -34,9 +35,10 @@ namespace strategy::communication {
 /**
  * @brief a conglomeration of the different request types.
  */
-using AgentRequest = std::variant<JoinWallRequest, TestRequest, PassRequest, ScorerRequest,
-                                  BallInTransitRequest, SeekerRequest, PositionRequest,
-                                  LeaveWallRequest, ResetScorerRequest, IncomingBallRequest>;
+using AgentRequest =
+    std::variant<JoinWallRequest, TestRequest, PassRequest, ScorerRequest, BallInTransitRequest,
+                 SeekerRequest, PositionRequest, LeaveWallRequest, ResetScorerRequest,
+                 IncomingBallRequest, PassReceivedRequest>;
 
 /**
  * @brief a conglomeration of the different response types.
@@ -159,6 +161,9 @@ struct RosConverter<strategy::communication::AgentRequest, rj_msgs::msg::AgentRe
         } else if (const auto* incoming_ball_request =
                        std::get_if<strategy::communication::IncomingBallRequest>(&from)) {
             result.incoming_ball_request.emplace_back(convert_to_ros(*incoming_ball_request));
+        } else if (const auto* pass_received_request =
+                       std::get_if<strategy::communication::PassReceivedRequest>(&from)) {
+            result.pass_received_request.emplace_back(convert_to_ros(*pass_received_request));
         } else {
             throw std::runtime_error("Invalid variant of AgentRequest");
         }
@@ -187,6 +192,8 @@ struct RosConverter<strategy::communication::AgentRequest, rj_msgs::msg::AgentRe
             result = convert_from_ros(from.reset_scorer_request.front());
         } else if (!from.incoming_ball_request.empty()) {
             result = convert_from_ros(from.incoming_ball_request.front());
+        } else if (!from.pass_received_request.empty()) {
+            result = convert_from_ros(from.pass_received_request.front());
         } else {
             throw std::runtime_error("Invalid variant of AgentRequest");
         }

src/rj_strategy/include/rj_strategy/agent/communication/pass_received_request.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <mutex>
+#include <string>
+#include <variant>
+#include <vector>
+
+#include <rj_common/time.hpp>
+#include <rj_convert/ros_convert.hpp>
+
+#include "rj_msgs/msg/pass_received_request.hpp"
+
+namespace strategy::communication {
+
+struct PassReceivedRequest {
+    uint32_t request_uid;
+    uint8_t from_robot_id;
+};
+
+bool operator==(const PassReceivedRequest& a, const PassReceivedRequest& b);
+void generate_uid(PassReceivedRequest& request);
+
+}  // namespace strategy::communication
+
+namespace rj_convert {
+
+template <>
+struct RosConverter<strategy::communication::PassReceivedRequest,
+                    rj_msgs::msg::PassReceivedRequest> {
+    static rj_msgs::msg::PassReceivedRequest to_ros(
+        const strategy::communication::PassReceivedRequest& from) {
+        rj_msgs::msg::PassReceivedRequest result;
+        result.request_uid = from.request_uid;
+        result.from_robot_id = from.from_robot_id;
+        return result;
+    }
+
+    static strategy::communication::PassReceivedRequest from_ros(
+        const rj_msgs::msg::PassReceivedRequest& from) {
+        return strategy::communication::PassReceivedRequest{
+            from.request_uid,
+            from.from_robot_id,
+        };
+    }
+};
+
+ASSOCIATE_CPP_ROS(strategy::communication::PassReceivedRequest, rj_msgs::msg::PassReceivedRequest);
+
+}  // namespace rj_convert

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

@@ -174,6 +174,15 @@ class Position {
      */
     virtual void send_pass_confirmation(u_int8_t target_robot);
 
+    /**
+     * @brief tell the passer that this robot has received/controlled the ball.
+     * Called by the receiver when it has the ball so the passer can leave
+     * PASSING_FINISHED and return to DEFAULT.
+     *
+     * @param passer_robot_id the robot that passed the ball (recipient of this message)
+     */
+    virtual void send_pass_received_to_passer(u_int8_t passer_robot_id);
+
     /**
      * @brief acknowledges the pass confirmation from another robot
      *

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

@@ -54,6 +54,7 @@ class Offense : public Position {
         POSSESSION_START,  // Try to shoot and send pass request
         POSSESSION,        // Holding the ball
         PASSING,           // Pass the ball
+        PASSING_FINISHED,  // Kicked the ball, waiting for receiver to confirm
         STEALING,          // Getting the ball
         RECEIVING_START,   // Facing the ball
         RECEIVING,         // Getting the ball from a pass
@@ -95,11 +96,13 @@ class Offense : public Position {
             case SEEKING:
                 return RJ::Seconds{-1};
             case POSSESSION:
-                return RJ::Seconds{-1};
+                return RJ::Seconds{5};
             case POSSESSION_START:
                 return RJ::Seconds{-1};
             case PASSING:
                 return RJ::Seconds{5};
+            case PASSING_FINISHED:
+                return RJ::Seconds{5};
             case STEALING:
                 return RJ::Seconds{10};
             case RECEIVING_START:
@@ -126,6 +129,8 @@ class Offense : public Position {
                 return "POSSESSION_START";
             case PASSING:
                 return "PASSING";
+            case PASSING_FINISHED:
+                return "PASSING_FINISHED";
             case STEALING:
                 return "STEALING";
             case RECEIVING_START:
@@ -187,6 +192,12 @@ class Offense : public Position {
     // Used to tell if an enemy is close enough to block a shot
     static constexpr double kEnemyTooCloseRadius{kStealBallRadius};
 
+    static constexpr double kMinPassDistance{0.5};
+
+    // If the ball hasn't left the passer within this window after the kick
+    // command fires, the kick is considered failed and both robots reset.
+    static constexpr RJ::Seconds kKickFailsafeTimeout{2.0};
+
     /* Utility functions for State or Task Calculation */
 
     /**
@@ -218,6 +229,12 @@ class Offense : public Position {
      * @param target_robot_shell the robot shell to check if open
      */
     bool check_if_open(int target_robot_shell);
+
+    /**
+     * @return whether or not a robot is capable of shooting from their pos
+     */
+    bool can_i_shoot() const;
+
     /**
      * @return the target (within the goal) that would be the most clear shot
      */
@@ -228,6 +245,19 @@ class Offense : public Position {
      */
     bool ball_in_red() const;
 
+    /**
+     * @return true when in PASSING_FINISHED and the kick failsafe timeout has
+     *         elapsed while the ball is still within possession range of this robot.
+     */
+    bool kick_failed() const;
+
+    /**
+     * @brief Notify the receiver that the kick failed so it can abort and
+     *        return to default behaviour. Uses the same PassReceivedRequest
+     *        message type that the receiver normally sends to the passer.
+     */
+    void send_kick_failed_to_receiver(u_int8_t receiver_robot_id);
+
     void broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding);
 
     std::unordered_map<int, rj_geometry::Point> seeker_points_;

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

@@ -110,6 +110,10 @@ class RobotFactoryPosition : public Position {
         current_position_->send_pass_confirmation(target_robot);
     }
 
+    void send_pass_received_to_passer(u_int8_t passer_robot_id) override {
+        current_position_->send_pass_received_to_passer(passer_robot_id);
+    }
+
     void set_override_position(const OverridingPositions& overriding_position);
 
 private:

src/rj_strategy/src/agent/communication.cpp

@@ -48,6 +48,10 @@ bool operator==(const IncomingBallRequest& a, const IncomingBallRequest& b) {
     return a.request_uid == b.request_uid;
 }
 
+bool operator==(const PassReceivedRequest& a, const PassReceivedRequest& b) {
+    return a.request_uid == b.request_uid;
+}
+
 bool operator==(const ScorerResponse& a, const ScorerResponse& b) {
     return a.response_uid == b.response_uid;
 }
@@ -150,6 +154,13 @@ void generate_uid(IncomingBallRequest& request) {
     request_uid_mutex.unlock();
 }
 
+void generate_uid(PassReceivedRequest& request) {
+    request_uid_mutex.lock();
+    request.request_uid = request_uid;
+    request_uid++;
+    request_uid_mutex.unlock();
+}
+
 void generate_uid(ScorerResponse& response) {
     response_uid_mutex.lock();
     response.response_uid = response_uid;