From 098bc09ca17f2107b0605165d00e5c62b24f6118 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 16 Jan 2024 19:36:43 -0500 Subject: [PATCH 1/3] Removed unnecessary changes and added comments --- .../strategy/agent/position/offense.cpp | 37 +++++++++++++++++-- .../strategy/agent/position/offense.hpp | 4 ++ .../strategy/agent/position/position.cpp | 24 +++++++++--- .../strategy/agent/position/position.hpp | 2 + 4 files changed, 58 insertions(+), 9 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 812f01bdb83..2c6c59fba30 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -29,8 +29,8 @@ Offense::State Offense::update_state() { 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 @@ Offense::State Offense::update_state() { 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 @@ std::optional Offense::state_to_task(RobotIntent intent) { 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 @@ std::optional Offense::state_to_task(RobotIntent intent) { 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,30 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( std::get_if(&request.request)) { communication::Acknowledge response = receive_reset_scorer_request(); comm_response.response = response; + } else if (const communication::PassRequest* pass_request = + std::get_if(&request.request)) { + //If the robot recieves a PassRequest, only process it if we are oppen + + rj_geometry::Point robot_position = world_state()->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point from_robot_position = 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 : 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; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index e09b513f192..a64f6dac123 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -49,6 +49,7 @@ class Offense : public Position { 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 * @@ -77,6 +80,7 @@ class Offense : public Position { communication::ScorerResponse receive_scorer_request( communication::ScorerRequest scorer_request); + /** * @brief This agent can go through the distance of every other offensive robot from the goal * to decide whether this robot should become the scorer. diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index fa51871ab76..2e795869fa4 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -116,11 +116,11 @@ 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(&request.request)) { - communication::PassResponse pass_response = receive_pass_request(*pass_request); - comm_response.response = pass_response; - } else if (const communication::IncomingBallRequest* incoming_ball_request = + // if (const communication::PassRequest* pass_request = + // std::get_if(&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(&request.request)) { communication::Acknowledge incoming_pass_acknowledge = acknowledge_pass(*incoming_ball_request); @@ -155,6 +155,19 @@ void Position::send_direct_pass_request(std::vector 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{}; @@ -162,7 +175,6 @@ communication::PassResponse Position::receive_pass_request( 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 diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 02f5226e222..308293d6175 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -129,6 +129,8 @@ class Position { */ void send_direct_pass_request(std::vector target_robots); + void broadcast_direct_pass_request(); + /** * @brief receives and handles a pass_request * From eb782f748768265233120c2cdbe04f620523688b Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 16 Jan 2024 21:32:36 -0500 Subject: [PATCH 2/3] Rebased to ros2 --- soccer/src/soccer/strategy/agent/position/offense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 2c6c59fba30..efb6c5d4383 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -216,15 +216,15 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( std::get_if(&request.request)) { //If the robot recieves a PassRequest, only process it if we are oppen - rj_geometry::Point robot_position = world_state()->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point from_robot_position = world_state()->get_robot(true, pass_request->from_robot_id).pose.position(); + 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 : world_state()->their_robots) { + 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)); From 28d7d82a43450e5b79538bffdbf2e4c27d357542 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 21 Jan 2024 19:35:17 -0500 Subject: [PATCH 3/3] Fix Code Style On direct-pass (#2158) automated style fixes Co-authored-by: sanatd33 --- .../strategy/agent/position/offense.cpp | 21 +++++++++++-------- .../strategy/agent/position/offense.hpp | 21 +++++++++---------- .../strategy/agent/position/position.cpp | 2 +- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index efb6c5d4383..49e8bea70bd 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -78,7 +78,7 @@ Offense::State Offense::update_state() { next_state = IDLING; } } else if (current_state_ == AWAITING_SEND_PASS) { - if (distance_to_ball < ball_lost_distance_){ + if (distance_to_ball < ball_lost_distance_) { Position::broadcast_direct_pass_request(); } } @@ -214,24 +214,27 @@ communication::PosAgentResponseWrapper Offense::receive_communication_request( comm_response.response = response; } else if (const communication::PassRequest* pass_request = std::get_if(&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(); + // 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 + // 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 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; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index a64f6dac123..2c13dacef8f 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -40,16 +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 - AWAITING_SEND_PASS, //is waiting to send a pass to someone else + 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(); @@ -80,7 +80,6 @@ class Offense : public Position { communication::ScorerResponse receive_scorer_request( communication::ScorerRequest scorer_request); - /** * @brief This agent can go through the distance of every other offensive robot from the goal * to decide whether this robot should become the scorer. diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 2e795869fa4..bf329b6ab15 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -121,7 +121,7 @@ communication::PosAgentResponseWrapper Position::receive_communication_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(&request.request)) { + std::get_if(&request.request)) { communication::Acknowledge incoming_pass_acknowledge = acknowledge_pass(*incoming_ball_request); comm_response.response = incoming_pass_acknowledge;