Direct pass

soccer/src/soccer/strategy/agent/position/offense.cpp

@@ -14,7 +14,7 @@
     return state_to_task(intent);
 }
 
 Offense::State Offense::update_state() {
     State next_state = current_state_;
     // handle transitions between current state
     WorldState* world_state = this->last_world_state_;
@@ -29,8 +29,8 @@
     double distance_to_ball = robot_position.dist_to(ball_position);
 
     if (current_state_ == IDLING) {
-        send_scorer_request();
-        next_state = SEARCHING;
+        // send_scorer_request();
+        next_state = AWAITING_SEND_PASS;
     } else if (current_state_ == SEARCHING) {
         if (scorer_) {
             next_state = STEALING;
@@ -77,6 +77,10 @@
         if (check_is_done()) {
             next_state = IDLING;
         }
+    } else if (current_state_ == AWAITING_SEND_PASS) {
+        if (distance_to_ball < ball_lost_distance_) {
+            Position::broadcast_direct_pass_request();
+        }
     }
 
     return next_state;
@@ -94,7 +98,6 @@
         intent.motion_command = empty_motion_cmd;
         return intent;
     } else if (current_state_ == PASSING) {
-        // attempt to pass the ball to the target robot
         rj_geometry::Point target_robot_pos =
             last_world_state_->get_robot(true, target_robot_id).pose.position();
         planning::LinearMotionInstant target{target_robot_pos};
@@ -174,6 +177,10 @@
             planning::MotionCommand{"path_target", current_location_instant, face_ball};
         intent.motion_command = face_ball_cmd;
         return intent;
+    } else if (current_state_ == AWAITING_SEND_PASS) {
+        auto empty_motion_cmd = planning::MotionCommand{};
+        intent.motion_command = empty_motion_cmd;
+        return intent;
     }
 
     // should be impossible to reach, but this is an EmptyMotionCommand
@@ -205,6 +212,33 @@
                    std::get_if<communication::ResetScorerRequest>(&request.request)) {
         communication::Acknowledge response = receive_reset_scorer_request();
         comm_response.response = response;
+    } else if (const communication::PassRequest* pass_request =
+                   std::get_if<communication::PassRequest>(&request.request)) {
+        // If the robot recieves a PassRequest, only process it if we are oppen
+
+        rj_geometry::Point robot_position =
+            last_world_state_->get_robot(true, robot_id_).pose.position();
+        rj_geometry::Point from_robot_position =
+            last_world_state_->get_robot(true, pass_request->from_robot_id).pose.position();
+        rj_geometry::Segment pass_path{from_robot_position, robot_position};
+        double min_robot_dist = 10000;
+        float min_path_dist = 10000;
+
+        // Calculates the minimum distance from the current robot to all other robots
+        // Also calculates the minimum distance from another robot to the passing line
+        for (auto bot : last_world_state_->their_robots) {
+            rj_geometry::Point opp_pos = bot.pose.position();
+            min_robot_dist = std::min(min_robot_dist, robot_position.dist_to(opp_pos));
+            min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos));
+        }
+
+        // If the current robot is far enough away from other robots and there are no other robots
+        // in the passing line, process the request Currently, max_receive_distance is used to
+        // determine when we are open, but this may need to change
+        if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) {
+            communication::PassResponse response = Position::receive_pass_request(*pass_request);
+            comm_response.response = response;
+        }
     }
 
     return comm_response;

soccer/src/soccer/strategy/agent/position/offense.hpp

@@ -40,15 +40,16 @@ class Offense : public Position {
     // TODO (Kevin): strategy design pattern for BallHandler/Receiver
 
     enum State {
-        IDLING,          // simply staying in place
-        SEARCHING,       // moving around on the field to get open
-        PASSING,         // physically kicking the ball towards another robot
-        PREPARING_SHOT,  // pivot around ball in preparation for shot
-        SHOOTING,        // physically kicking the ball towards the net
-        RECEIVING,       // physically intercepting the ball from a pass (gets possession)
-        STEALING,        // attempting to intercept the ball from the other team
-        FACING,          // turning to face the ball
-        SCORER,          // overrides everything and will attempt to steal the bal and shoot it
+        IDLING,              // simply staying in place
+        SEARCHING,           // moving around on the field to get open
+        PASSING,             // physically kicking the ball towards another robot
+        PREPARING_SHOT,      // pivot around ball in preparation for shot
+        SHOOTING,            // physically kicking the ball towards the net
+        RECEIVING,           // physically intercepting the ball from a pass (gets possession)
+        STEALING,            // attempting to intercept the ball from the other team
+        FACING,              // turning to face the ball
+        SCORER,              // overrides everything and will attempt to steal the bal and shoot it
+        AWAITING_SEND_PASS,  // is waiting to send a pass to someone else
     };
 
     State update_state();
@@ -61,6 +62,8 @@ class Offense : public Position {
     bool scorer_ = false;
     bool last_scorer_ = false;
 
+    communication::PassResponse receive_pass_request(communication::PassRequest pass_request);
+
     /**
      * @brief Send request to the other robots to see if this robot should be the scorer
      *

soccer/src/soccer/strategy/agent/position/position.cpp

@@ -116,12 +116,12 @@ void Position::receive_communication_response(communication::AgentPosResponseWra
 communication::PosAgentResponseWrapper Position::receive_communication_request(
     communication::AgentPosRequestWrapper request) {
     communication::PosAgentResponseWrapper comm_response{};
-    if (const communication::PassRequest* pass_request =
-            std::get_if<communication::PassRequest>(&request.request)) {
-        communication::PassResponse pass_response = receive_pass_request(*pass_request);
-        comm_response.response = pass_response;
-    } else if (const communication::IncomingBallRequest* incoming_ball_request =
-                   std::get_if<communication::IncomingBallRequest>(&request.request)) {
+    // if (const communication::PassRequest* pass_request =
+    //         std::get_if<communication::PassRequest>(&request.request)) {
+    //     communication::PassResponse pass_response = receive_pass_request(*pass_request);
+    //     comm_response.response = pass_response;
+    if (const communication::IncomingBallRequest* incoming_ball_request =
+            std::get_if<communication::IncomingBallRequest>(&request.request)) {
         communication::Acknowledge incoming_pass_acknowledge =
             acknowledge_pass(*incoming_ball_request);
         comm_response.response = incoming_pass_acknowledge;
@@ -155,14 +155,26 @@ void Position::send_direct_pass_request(std::vector<u_int8_t> target_robots) {
     communication_request_ = communication_request;
 }
 
+void Position::broadcast_direct_pass_request() {
+    communication::PassRequest pass_request{};
+    communication::generate_uid(pass_request);
+    pass_request.direct = true;
+    pass_request.from_robot_id = robot_id_;
+
+    communication::PosAgentRequestWrapper communication_request{};
+    communication_request.request = pass_request;
+    communication_request.urgent = true;
+    communication_request.broadcast = true;
+    communication_request_ = communication_request;
+}
+
 communication::PassResponse Position::receive_pass_request(
     communication::PassRequest pass_request) {
     communication::PassResponse pass_response{};
     communication::generate_uid(pass_response);
 
     if (pass_request.direct) {
         // Handle direct pass request
-        // TODO: Make this rely on actually being open
         pass_response.direct_open = true;
     } else {
         // TODO: Handle indirect pass request

soccer/src/soccer/strategy/agent/position/position.hpp

@@ -129,6 +129,8 @@ class Position {
      */
     void send_direct_pass_request(std::vector<u_int8_t> target_robots);
 
+    void broadcast_direct_pass_request();
+
     /**
      * @brief receives and handles a pass_request
      *