From 5183dcc1fad45fb2b8f3dc06094f3db1681ee016 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 16 Jan 2024 21:30:12 -0500 Subject: [PATCH 01/22] Fixed heuristics for seeking --- .../strategy/agent/position/offense.cpp | 231 +++++++++++++++++- .../strategy/agent/position/offense.hpp | 20 ++ .../strategy/agent/position/position.cpp | 24 +- .../strategy/agent/position/position.hpp | 2 + 4 files changed, 267 insertions(+), 10 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 812f01bdb83..2d4494a634f 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -29,8 +29,12 @@ 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(); + if (robot_id_ == 4) { + next_state = AWAITING_SEND_PASS; + } else { + next_state = SEEKING; + } } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -58,7 +62,7 @@ Offense::State Offense::update_state() { } } else if (current_state_ == RECEIVING) { // transition to idling if we are close enough to the ball - if (distance_to_ball < ball_receive_distance_) { + if (distance_to_ball < 2*ball_receive_distance_) { next_state = IDLING; } } else if (current_state_ == STEALING) { @@ -77,7 +81,20 @@ Offense::State Offense::update_state() { if (check_is_done()) { next_state = IDLING; } + } else if (current_state_ == AWAITING_SEND_PASS) { + auto cur = std::chrono::high_resolution_clock::now(); + auto diff = cur - start; + if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2){ + Position::broadcast_direct_pass_request(); + start = std::chrono::high_resolution_clock::now(); + } + } else if (current_state_ == SEEKING) { + // if the ball comes close to it while it's trying to seek, it should no longer be trying to seek + if (distance_to_ball < ball_receive_distance_) { + // next_state = IDLING; + } } + return next_state; } @@ -94,7 +111,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,12 +190,195 @@ 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; + } else if (current_state_ == SEEKING) { + rj_geometry::Point current_loc = last_world_state_->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point curr_target = intent.motion_command.target.position; + if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { + target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); + } + planning::PathTargetFaceOption face_option = planning::FaceBall{}; + bool ignore_ball = false; + // SPDLOG_INFO("Robot ID: {}, Target Pt: ({}, {})", robot_id_, target_pt.x(), target_pt.y()); + planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; + intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + return intent; } // should be impossible to reach, but this is an EmptyMotionCommand return std::nullopt; } +rj_geometry::Point Offense::get_open_point(const WorldState* world_state, + rj_geometry::Point current_loc, + FieldDimensions field_dimensions) { + return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); +} + +rj_geometry::Point Offense::calculate_open_point(double current_prec, double min_prec, + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions) { + while (current_prec > min_prec) { + rj_geometry::Point ball_pos = world_state->ball.position; + rj_geometry::Point min = current_point; + double min_val = max_los(ball_pos, current_point, world_state); + double curr_val{}; + // Points in a current_prec radius of the current point, at 45 degree intervals + std::vector check_points{ + correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, + field_dimensions)}; + + for (auto point : check_points) { + curr_val = max_los(ball_pos, point, world_state); + if (curr_val < min_val) { + min_val = curr_val; + min = point; + } + SPDLOG_INFO("I am {}, possible pt ({}, {}), val {}", robot_id_, point.x(), point.y(), curr_val); + } + current_prec *= 0.5; + current_point = min; + } + return current_point; +} + +rj_geometry::Point Offense::random_noise(double prec) { + double x = (double)rand() / RAND_MAX * prec; + double y = (double)rand() / RAND_MAX * prec; + return rj_geometry::Point{x, y}; +} + +rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { + double BORDER_BUFFER = .2; + double x = p.x(); + double y = p.y(); + + // X Border + if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { + x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; + } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { + x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; + } + + // Y Border + if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { + y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; + } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { + y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; + } + + // Goalie Boxes + if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { + if (y > 4.5) { + y = 8.0 - BORDER_BUFFER; + } else { + y = 1.0 + BORDER_BUFFER; + } + + if (x > .5) { + x = 1.0 + BORDER_BUFFER; + } else { + x = -1.0 - BORDER_BUFFER; + } + } + + // Assigns robots to horizontal thirds + if (robot_id_ == 1) { + // Assign left + if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - + BORDER_BUFFER; + } + } else if (robot_id_ == 2) { + // Assign right + if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + + BORDER_BUFFER; + } + } else { + // Assign middle + if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + + BORDER_BUFFER; + } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - + BORDER_BUFFER; + } + } + + return rj_geometry::Point(x, y); +} + +double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state) { + + + rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; + if (goal_box.contains_point(current_point)) { + return 10000000; + } + + double max = 0; + double curr_dp; + for (auto robot : world_state->their_robots) { + curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + rj_geometry::Segment pass_path{ball_pos, current_point}; + double min_robot_dist = 10000; + float min_path_dist = 10000; + for (auto bot : world_state->their_robots) { + rj_geometry::Point opp_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); + } + + for (auto bot : world_state->our_robots) { + rj_geometry::Point ally_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); + } + + min_path_dist = 0.1 / min_path_dist; + min_robot_dist = 0.1 / min_robot_dist; + + for (auto robot : world_state->our_robots) { + curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + // Additional heuristics for calculating optimal point + double ball_proximity_loss = (current_point - ball_pos).mag() * .002; + double goal_distance_loss = (9.0 - current_point.y()) * .008; + + return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; +} + void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) { Position::receive_communication_response(response); @@ -205,6 +404,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 = 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; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index e09b513f192..5c7b0d7fe6c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -34,6 +35,7 @@ class Offense : public Position { void derived_acknowledge_ball_in_transit() override; private: + rj_geometry::Point target_pt{0.0, 0.0}; bool kicking_{true}; std::optional derived_get_task(RobotIntent intent) override; @@ -49,6 +51,8 @@ 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 + SEEKING, // is trying to get open }; State update_state(); @@ -61,6 +65,21 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; + rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, + FieldDimensions field_dimensions); + + rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions); + rj_geometry::Point random_noise(double prec); + rj_geometry::Point calculate_open_point(double, double, rj_geometry::Point, + const WorldState* world_state, + FieldDimensions field_dimensions); + double max_los(rj_geometry::Point, rj_geometry::Point, const WorldState* world_state); + + int seeker_pos_; + std::string offense_type_; + + std::chrono::time_point start = std::chrono::high_resolution_clock::now(); + /** * @brief Send request to the other robots to see if this robot should be the scorer * @@ -77,6 +96,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 03d7fa98f4a79c61f739f61c36ee5156e00434cc Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 16 Jan 2024 21:31:17 -0500 Subject: [PATCH 02/22] Removed log statement --- soccer/src/soccer/strategy/agent/position/offense.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 2d4494a634f..3140c2ee52a 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -252,7 +252,6 @@ rj_geometry::Point Offense::calculate_open_point(double current_prec, double min min_val = curr_val; min = point; } - SPDLOG_INFO("I am {}, possible pt ({}, {}), val {}", robot_id_, point.x(), point.y(), curr_val); } current_prec *= 0.5; current_point = min; From 8a868b6883c1e7ea2f1f5790aad6497fa5b0efb2 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 21 Jan 2024 19:23:44 -0500 Subject: [PATCH 03/22] Added comments to offense.cpp --- .../src/soccer/strategy/agent/position/offense.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3140c2ee52a..09633be46d0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -195,14 +195,15 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { + //Determine target position for seeking rj_geometry::Point current_loc = last_world_state_->get_robot(true, robot_id_).pose.position(); rj_geometry::Point curr_target = intent.motion_command.target.position; + //Only look for a new position if we are done with current movement if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); } planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; - // SPDLOG_INFO("Robot ID: {}, Target Pt: ({}, {})", robot_id_, target_pt.x(), target_pt.y()); planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; return intent; @@ -328,13 +329,16 @@ rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state) { + //Determines 'how good' a point is + //A higher value is a worse point - + //Does not go into the goalie boxes rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; if (goal_box.contains_point(current_point)) { return 10000000; } + //Line of Sight Heuristic double max = 0; double curr_dp; for (auto robot : world_state->their_robots) { @@ -345,6 +349,8 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ } } + //Whether the path from ball to the point is blocked + //Same logic in passing to check if target is open rj_geometry::Segment pass_path{ball_pos, current_point}; double min_robot_dist = 10000; float min_path_dist = 10000; @@ -363,6 +369,7 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ min_path_dist = 0.1 / min_path_dist; min_robot_dist = 0.1 / min_robot_dist; + //More Line of Sight Heuristics for (auto robot : world_state->our_robots) { curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; @@ -375,6 +382,7 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ double ball_proximity_loss = (current_point - ball_pos).mag() * .002; double goal_distance_loss = (9.0 - current_point.y()) * .008; + //Final evaluation return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; } From 85ecac3b06d689619b11ed96a839d85ccc4b16d9 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 21 Jan 2024 19:26:57 -0500 Subject: [PATCH 04/22] Remove testing code --- soccer/src/soccer/strategy/agent/position/offense.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 09633be46d0..3581999213c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,11 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { // send_scorer_request(); - if (robot_id_ == 4) { - next_state = AWAITING_SEND_PASS; - } else { - next_state = SEEKING; - } + next_state = SEEKING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -82,6 +78,7 @@ Offense::State Offense::update_state() { next_state = IDLING; } } else if (current_state_ == AWAITING_SEND_PASS) { + //Only sends a pass request every 0.2 seconds auto cur = std::chrono::high_resolution_clock::now(); auto diff = cur - start; if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2){ @@ -91,7 +88,7 @@ Offense::State Offense::update_state() { } else if (current_state_ == SEEKING) { // if the ball comes close to it while it's trying to seek, it should no longer be trying to seek if (distance_to_ball < ball_receive_distance_) { - // next_state = IDLING; + // next_state = RECEIVING; } } From 1cc87d9f4398cdea88e84775ea549bffd83a89c3 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:34:52 -0500 Subject: [PATCH 05/22] Fix Code Style On seeking-test (#2161) automated style fixes Co-authored-by: sanatd33 --- .../strategy/agent/position/offense.cpp | 71 ++++++++++--------- .../strategy/agent/position/offense.hpp | 33 ++++----- .../strategy/agent/position/position.cpp | 2 +- 3 files changed, 56 insertions(+), 50 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3581999213c..534e9a64cfb 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -58,7 +58,7 @@ Offense::State Offense::update_state() { } } else if (current_state_ == RECEIVING) { // transition to idling if we are close enough to the ball - if (distance_to_ball < 2*ball_receive_distance_) { + if (distance_to_ball < 2 * ball_receive_distance_) { next_state = IDLING; } } else if (current_state_ == STEALING) { @@ -78,20 +78,20 @@ Offense::State Offense::update_state() { next_state = IDLING; } } else if (current_state_ == AWAITING_SEND_PASS) { - //Only sends a pass request every 0.2 seconds + // Only sends a pass request every 0.2 seconds auto cur = std::chrono::high_resolution_clock::now(); auto diff = cur - start; - if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2){ + if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2) { Position::broadcast_direct_pass_request(); start = std::chrono::high_resolution_clock::now(); } - } else if (current_state_ == SEEKING) { - // if the ball comes close to it while it's trying to seek, it should no longer be trying to seek + } else if (current_state_ == SEEKING) { + // if the ball comes close to it while it's trying to seek, it should no longer be trying to + // seek if (distance_to_ball < ball_receive_distance_) { // next_state = RECEIVING; } } - return next_state; } @@ -192,17 +192,19 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { - //Determine target position for seeking - rj_geometry::Point current_loc = last_world_state_->get_robot(true, robot_id_).pose.position(); + // Determine target position for seeking + rj_geometry::Point current_loc = + last_world_state_->get_robot(true, robot_id_).pose.position(); rj_geometry::Point curr_target = intent.motion_command.target.position; - //Only look for a new position if we are done with current movement + // Only look for a new position if we are done with current movement if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); } planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + intent.motion_command = + planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; return intent; } @@ -211,15 +213,15 @@ std::optional Offense::state_to_task(RobotIntent intent) { } rj_geometry::Point Offense::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, - FieldDimensions field_dimensions) { + rj_geometry::Point current_loc, + FieldDimensions field_dimensions) { return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); } rj_geometry::Point Offense::calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - FieldDimensions field_dimensions) { + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions) { while (current_prec > min_prec) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point min = current_point; @@ -325,17 +327,17 @@ rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions } double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { - //Determines 'how good' a point is - //A higher value is a worse point + const WorldState* world_state) { + // Determines 'how good' a point is + // A higher value is a worse point - //Does not go into the goalie boxes + // Does not go into the goalie boxes rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; if (goal_box.contains_point(current_point)) { return 10000000; } - //Line of Sight Heuristic + // Line of Sight Heuristic double max = 0; double curr_dp; for (auto robot : world_state->their_robots) { @@ -346,8 +348,8 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ } } - //Whether the path from ball to the point is blocked - //Same logic in passing to check if target is open + // Whether the path from ball to the point is blocked + // Same logic in passing to check if target is open rj_geometry::Segment pass_path{ball_pos, current_point}; double min_robot_dist = 10000; float min_path_dist = 10000; @@ -362,11 +364,11 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); } - + min_path_dist = 0.1 / min_path_dist; min_robot_dist = 0.1 / min_robot_dist; - //More Line of Sight Heuristics + // More Line of Sight Heuristics for (auto robot : world_state->our_robots) { curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; @@ -379,7 +381,7 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ double ball_proximity_loss = (current_point - ball_pos).mag() * .002; double goal_distance_loss = (9.0 - current_point.y()) * .008; - //Final evaluation + // Final evaluation return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; } @@ -410,24 +412,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 5c7b0d7fe6c..9202b71c398 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include +#include #include #include @@ -42,17 +42,17 @@ 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 - SEEKING, // is trying to get open + 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 + SEEKING, // is trying to get open }; State update_state(); @@ -65,8 +65,9 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; - rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, - FieldDimensions field_dimensions); + rj_geometry::Point get_open_point(const WorldState* world_state, + rj_geometry::Point current_position, + FieldDimensions field_dimensions); rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions); rj_geometry::Point random_noise(double prec); @@ -78,7 +79,8 @@ class Offense : public Position { int seeker_pos_; std::string offense_type_; - std::chrono::time_point start = std::chrono::high_resolution_clock::now(); + std::chrono::time_point start = + std::chrono::high_resolution_clock::now(); /** * @brief Send request to the other robots to see if this robot should be the scorer @@ -96,7 +98,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; From 545322f42994104e3c0cdbd969f072eacf751ac7 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 23 Jan 2024 20:06:27 -0500 Subject: [PATCH 06/22] Created Role Interface for Seeking --- soccer/src/soccer/CMakeLists.txt | 1 + .../strategy/agent/position/offense.cpp | 190 +---------------- .../strategy/agent/position/offense.hpp | 14 +- .../strategy/agent/position/seeking.cpp | 195 ++++++++++++++++++ .../strategy/agent/position/seeking.hpp | 100 +++++++++ 5 files changed, 302 insertions(+), 198 deletions(-) create mode 100644 soccer/src/soccer/strategy/agent/position/seeking.cpp create mode 100644 soccer/src/soccer/strategy/agent/position/seeking.hpp diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 9f93219dc06..9eb09b970fe 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -81,6 +81,7 @@ set(ROBOCUP_LIB_SRC strategy/agent/position/offense.cpp strategy/agent/position/defense.cpp strategy/agent/position/waller.cpp + strategy/agent/position/seeking.cpp strategy/agent/position/goal_kicker.cpp strategy/agent/position/penalty_player.cpp ) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 534e9a64cfb..7609cce0771 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -192,199 +192,17 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { - // Determine target position for seeking - rj_geometry::Point current_loc = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point curr_target = intent.motion_command.target.position; - // Only look for a new position if we are done with current movement - if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { - target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); + //Only get a new target position if we have reached our target + if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { + Seeking seeker{robot_id_}; + return seeker.get_task(intent, last_world_state_, this->field_dimensions_); } - planning::PathTargetFaceOption face_option = planning::FaceBall{}; - bool ignore_ball = false; - planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = - planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; - return intent; } // should be impossible to reach, but this is an EmptyMotionCommand return std::nullopt; } -rj_geometry::Point Offense::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, - FieldDimensions field_dimensions) { - return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); -} - -rj_geometry::Point Offense::calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - FieldDimensions field_dimensions) { - while (current_prec > min_prec) { - rj_geometry::Point ball_pos = world_state->ball.position; - rj_geometry::Point min = current_point; - double min_val = max_los(ball_pos, current_point, world_state); - double curr_val{}; - // Points in a current_prec radius of the current point, at 45 degree intervals - std::vector check_points{ - correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), - correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), - correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), - correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), - correct_point( - current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, - field_dimensions)}; - - for (auto point : check_points) { - curr_val = max_los(ball_pos, point, world_state); - if (curr_val < min_val) { - min_val = curr_val; - min = point; - } - } - current_prec *= 0.5; - current_point = min; - } - return current_point; -} - -rj_geometry::Point Offense::random_noise(double prec) { - double x = (double)rand() / RAND_MAX * prec; - double y = (double)rand() / RAND_MAX * prec; - return rj_geometry::Point{x, y}; -} - -rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { - double BORDER_BUFFER = .2; - double x = p.x(); - double y = p.y(); - - // X Border - if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { - x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; - } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { - x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; - } - - // Y Border - if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { - y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; - } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { - y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; - } - - // Goalie Boxes - if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { - if (y > 4.5) { - y = 8.0 - BORDER_BUFFER; - } else { - y = 1.0 + BORDER_BUFFER; - } - - if (x > .5) { - x = 1.0 + BORDER_BUFFER; - } else { - x = -1.0 - BORDER_BUFFER; - } - } - - // Assigns robots to horizontal thirds - if (robot_id_ == 1) { - // Assign left - if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { - x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - - BORDER_BUFFER; - } - } else if (robot_id_ == 2) { - // Assign right - if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { - x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + - BORDER_BUFFER; - } - } else { - // Assign middle - if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { - x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + - BORDER_BUFFER; - } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { - x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - - BORDER_BUFFER; - } - } - - return rj_geometry::Point(x, y); -} - -double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { - // Determines 'how good' a point is - // A higher value is a worse point - - // Does not go into the goalie boxes - rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; - if (goal_box.contains_point(current_point)) { - return 10000000; - } - - // Line of Sight Heuristic - double max = 0; - double curr_dp; - for (auto robot : world_state->their_robots) { - curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); - curr_dp *= curr_dp; - if (curr_dp > max) { - max = curr_dp; - } - } - - // Whether the path from ball to the point is blocked - // Same logic in passing to check if target is open - rj_geometry::Segment pass_path{ball_pos, current_point}; - double min_robot_dist = 10000; - float min_path_dist = 10000; - for (auto bot : world_state->their_robots) { - rj_geometry::Point opp_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); - } - - for (auto bot : world_state->our_robots) { - rj_geometry::Point ally_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); - } - - min_path_dist = 0.1 / min_path_dist; - min_robot_dist = 0.1 / min_robot_dist; - - // More Line of Sight Heuristics - for (auto robot : world_state->our_robots) { - curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); - curr_dp *= curr_dp; - if (curr_dp > max) { - max = curr_dp; - } - } - - // Additional heuristics for calculating optimal point - double ball_proximity_loss = (current_point - ball_pos).mag() * .002; - double goal_distance_loss = (9.0 - current_point.y()) * .008; - - // Final evaluation - return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; -} - void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) { Position::receive_communication_response(response); diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 9202b71c398..81ea59eede0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -15,6 +15,8 @@ #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" +#include "seeking.hpp" + namespace strategy { /* @@ -35,7 +37,6 @@ class Offense : public Position { void derived_acknowledge_ball_in_transit() override; private: - rj_geometry::Point target_pt{0.0, 0.0}; bool kicking_{true}; std::optional derived_get_task(RobotIntent intent) override; @@ -65,17 +66,6 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; - rj_geometry::Point get_open_point(const WorldState* world_state, - rj_geometry::Point current_position, - FieldDimensions field_dimensions); - - rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions); - rj_geometry::Point random_noise(double prec); - rj_geometry::Point calculate_open_point(double, double, rj_geometry::Point, - const WorldState* world_state, - FieldDimensions field_dimensions); - double max_los(rj_geometry::Point, rj_geometry::Point, const WorldState* world_state); - int seeker_pos_; std::string offense_type_; diff --git a/soccer/src/soccer/strategy/agent/position/seeking.cpp b/soccer/src/soccer/strategy/agent/position/seeking.cpp new file mode 100644 index 00000000000..3b9a060b229 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/seeking.cpp @@ -0,0 +1,195 @@ +#include "seeking.hpp" + +namespace strategy { + +Seeking::Seeking(int robot_id) { + robot_id_ = robot_id; +} + +std::optional Seeking::get_task(RobotIntent intent, const WorldState* last_world_state, + FieldDimensions field_dimensions) { + // Determine target position for seeking + rj_geometry::Point current_loc = + last_world_state->get_robot(true, robot_id_).pose.position(); + + target_pt = get_open_point(last_world_state, current_loc, field_dimensions); + + planning::PathTargetFaceOption face_option = planning::FaceBall{}; + bool ignore_ball = false; + planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; + intent.motion_command = + planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + + return intent; +} + +rj_geometry::Point Seeking::get_open_point(const WorldState* world_state, + rj_geometry::Point current_loc, + FieldDimensions field_dimensions) { + return Seeking::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); +} + +rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min_prec, + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions) { + while (current_prec > min_prec) { + rj_geometry::Point ball_pos = world_state->ball.position; + rj_geometry::Point min = current_point; + double min_val = eval_point(ball_pos, current_point, world_state); + double curr_val{}; + // Points in a current_prec radius of the current point, at 45 degree intervals + std::vector check_points{ + correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, + field_dimensions)}; + + + // Finds the best point out of the ones checked + for (auto point : check_points) { + curr_val = eval_point(ball_pos, point, world_state); + if (curr_val < min_val) { + min_val = curr_val; + min = point; + } + } + current_prec *= 0.5; + current_point = min; + } + return current_point; +} + +rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { + double BORDER_BUFFER = .2; + double x = p.x(); + double y = p.y(); + + // X Border + if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { + x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; + } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { + x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; + } + + // Y Border + if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { + y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; + } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { + y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; + } + + // Goalie Boxes + if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { + if (y > 4.5) { + y = 8.0 - BORDER_BUFFER; + } else { + y = 1.0 + BORDER_BUFFER; + } + + if (x > .5) { + x = 1.0 + BORDER_BUFFER; + } else { + x = -1.0 - BORDER_BUFFER; + } + } + + // Assigns robots to horizontal thirds + if (robot_id_ == 1) { + // Assign left + if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - + BORDER_BUFFER; + } + } else if (robot_id_ == 2) { + // Assign right + if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + + BORDER_BUFFER; + } + } else { + // Assign middle + if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + + BORDER_BUFFER; + } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - + BORDER_BUFFER; + } + } + + return rj_geometry::Point(x, y); +} + +double Seeking::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state) { + // Determines 'how good' a point is + // A higher value is a worse point + + // Does not go into the goalie boxes + rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; + if (goal_box.contains_point(current_point)) { + return 10000000; + } + + // Line of Sight Heuristic + double max = 0; + double curr_dp; + for (auto robot : world_state->their_robots) { + curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + // Whether the path from ball to the point is blocked + // Same logic in passing to check if target is open + rj_geometry::Segment pass_path{ball_pos, current_point}; + double min_robot_dist = 10000; + float min_path_dist = 10000; + for (auto bot : world_state->their_robots) { + rj_geometry::Point opp_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); + } + + for (auto bot : world_state->our_robots) { + rj_geometry::Point ally_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); + } + + min_path_dist = 0.1 / min_path_dist; + min_robot_dist = 0.1 / min_robot_dist; + + // More Line of Sight Heuristics + for (auto robot : world_state->our_robots) { + curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + // Additional heuristics for calculating optimal point + double ball_proximity_loss = (current_point - ball_pos).mag() * .002; + double goal_distance_loss = (9.0 - current_point.y()) * .008; + + // Final evaluation + return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; +} + +} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/seeking.hpp b/soccer/src/soccer/strategy/agent/position/seeking.hpp new file mode 100644 index 00000000000..7b90ef07b41 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/seeking.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include + +#include +#include +#include + +#include + +#include "planning/instant.hpp" +#include "position.hpp" +#include "rj_common/field_dimensions.hpp" +#include "rj_common/time.hpp" +#include "rj_constants/constants.hpp" +#include "rj_geometry/geometry_conversions.hpp" +#include "rj_geometry/point.hpp" +#include "role_interface.hpp" + +namespace strategy { + +/* + * The Seeking role provides the implementation for a offensive robot that + * is trying to get open, so that they can receive a pass + */ +class Seeking : public RoleInterface { +public: + Seeking(int robot_id); + ~Seeking() = default; + + /** + * @brief Returns a seeker behavior which aims to get open + * + * @param intent The RobotIntent to add the movement to + * @param world_state The current WorldState + * @param field_dimensions The dimensions of the field + * + * @return [RobotIntent with next target point for the robot] + */ + std::optional get_task(RobotIntent intent, const WorldState* world_state, + FieldDimensions field_dimensions) override; + +private: + // The seeker's id + int robot_id_; + // The taret point to move to + rj_geometry::Point target_pt{0.0, 0.0}; + + /** + * @brief Returns the point which is most 'open' + * + * @param world_state The current WorldState + * @param current_position The current position of the seeker + * @param field_dimensions The dimensions of the field + * + * @return rj_geometry::Point The target point + */ + rj_geometry::Point get_open_point(const WorldState* world_state, + rj_geometry::Point current_position, + FieldDimensions field_dimensions); + + /** + * @brief Calculates which point is the best by iteratively searching around the robot + * + * @param current_prec A double that represents how far away to look from the robot + * @param min_prec A double that represents the minimum distance to look from the robot + * @param current_point The robot's current position + * @param world_state The current WorldState + * @param field_dimensions The dimensions of the field + * + * @return rj_geometry::Point The best point found + */ + rj_geometry::Point calculate_open_point(double current_prec, double min_prec, + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions); + + /** + * @brief Corrects the point to be within the field + * + * @param point The point to correct + * @param field_dimensions The dimensions of the field + * + * @return rj_geometry::Point The corrected point + */ + rj_geometry::Point correct_point(rj_geometry::Point point, FieldDimensions field_dimensions); + + /** + * @brief Calculates how 'good' a target point is + * + * @param ball_pos The current position of the ball + * @param current_point The point that is being evaluated + * @param world_state The current world state + * + * @return double The evaluation of that target point + */ + double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state); +}; + +} // namespace strategy From 580b39d27e561c07cf9effb051b1d81831f78a90 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 23 Jan 2024 20:13:16 -0500 Subject: [PATCH 07/22] Fix Code Style On seeking-test (#2167) automated style fixes Co-authored-by: sanatd33 --- .../strategy/agent/position/offense.cpp | 5 ++-- .../strategy/agent/position/offense.hpp | 1 - .../strategy/agent/position/seeking.cpp | 17 ++++------- .../strategy/agent/position/seeking.hpp | 29 ++++++++++--------- 4 files changed, 24 insertions(+), 28 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index cfbbe76a685..7e923f8fdd0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -188,8 +188,9 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { - //Only get a new target position if we have reached our target - if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { + // Only get a new target position if we have reached our target + if (check_is_done() || + last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { Seeking seeker{robot_id_}; return seeker.get_task(intent, last_world_state_, this->field_dimensions_); } diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index f5b99221b3a..adc5d8ec3a3 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -14,7 +14,6 @@ #include "rj_common/time.hpp" #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" - #include "seeking.hpp" namespace strategy { diff --git a/soccer/src/soccer/strategy/agent/position/seeking.cpp b/soccer/src/soccer/strategy/agent/position/seeking.cpp index 3b9a060b229..946019697f1 100644 --- a/soccer/src/soccer/strategy/agent/position/seeking.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeking.cpp @@ -2,24 +2,20 @@ namespace strategy { -Seeking::Seeking(int robot_id) { - robot_id_ = robot_id; -} +Seeking::Seeking(int robot_id) { robot_id_ = robot_id; } std::optional Seeking::get_task(RobotIntent intent, const WorldState* last_world_state, - FieldDimensions field_dimensions) { + FieldDimensions field_dimensions) { // Determine target position for seeking - rj_geometry::Point current_loc = - last_world_state->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point current_loc = last_world_state->get_robot(true, robot_id_).pose.position(); target_pt = get_open_point(last_world_state, current_loc, field_dimensions); planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = - planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; - + intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + return intent; } @@ -57,7 +53,6 @@ rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, field_dimensions)}; - // Finds the best point out of the ones checked for (auto point : check_points) { curr_val = eval_point(ball_pos, point, world_state); @@ -134,7 +129,7 @@ rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions } double Seeking::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { + const WorldState* world_state) { // Determines 'how good' a point is // A higher value is a worse point diff --git a/soccer/src/soccer/strategy/agent/position/seeking.hpp b/soccer/src/soccer/strategy/agent/position/seeking.hpp index 7b90ef07b41..19d5be5f088 100644 --- a/soccer/src/soccer/strategy/agent/position/seeking.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeking.hpp @@ -34,7 +34,7 @@ class Seeking : public RoleInterface { * @param intent The RobotIntent to add the movement to * @param world_state The current WorldState * @param field_dimensions The dimensions of the field - * + * * @return [RobotIntent with next target point for the robot] */ std::optional get_task(RobotIntent intent, const WorldState* world_state, @@ -48,28 +48,28 @@ class Seeking : public RoleInterface { /** * @brief Returns the point which is most 'open' - * + * * @param world_state The current WorldState * @param current_position The current position of the seeker * @param field_dimensions The dimensions of the field - * + * * @return rj_geometry::Point The target point - */ + */ rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, FieldDimensions field_dimensions); /** * @brief Calculates which point is the best by iteratively searching around the robot - * + * * @param current_prec A double that represents how far away to look from the robot * @param min_prec A double that represents the minimum distance to look from the robot * @param current_point The robot's current position * @param world_state The current WorldState * @param field_dimensions The dimensions of the field - * + * * @return rj_geometry::Point The best point found - */ + */ rj_geometry::Point calculate_open_point(double current_prec, double min_prec, rj_geometry::Point current_point, const WorldState* world_state, @@ -77,24 +77,25 @@ class Seeking : public RoleInterface { /** * @brief Corrects the point to be within the field - * + * * @param point The point to correct * @param field_dimensions The dimensions of the field - * + * * @return rj_geometry::Point The corrected point - */ + */ rj_geometry::Point correct_point(rj_geometry::Point point, FieldDimensions field_dimensions); /** * @brief Calculates how 'good' a target point is - * + * * @param ball_pos The current position of the ball * @param current_point The point that is being evaluated * @param world_state The current world state - * + * * @return double The evaluation of that target point - */ - double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state); + */ + double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state); }; } // namespace strategy From 41df6adebf57bce544df4836ed8d34a5720c497a Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 28 Jan 2024 20:37:47 -0500 Subject: [PATCH 08/22] Fixed warning messages --- soccer/src/soccer/CMakeLists.txt | 2 +- .../strategy/agent/position/offense.cpp | 2 +- .../strategy/agent/position/offense.hpp | 2 +- .../position/{seeking.cpp => seeker.cpp} | 28 +++++++++---------- .../position/{seeking.hpp => seeker.hpp} | 22 +++++++++------ 5 files changed, 30 insertions(+), 26 deletions(-) rename soccer/src/soccer/strategy/agent/position/{seeking.cpp => seeker.cpp} (85%) rename soccer/src/soccer/strategy/agent/position/{seeking.hpp => seeker.hpp} (79%) diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 9eb09b970fe..f0fbf689c1b 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -81,7 +81,7 @@ set(ROBOCUP_LIB_SRC strategy/agent/position/offense.cpp strategy/agent/position/defense.cpp strategy/agent/position/waller.cpp - strategy/agent/position/seeking.cpp + strategy/agent/position/seeker.cpp strategy/agent/position/goal_kicker.cpp strategy/agent/position/penalty_player.cpp ) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 7e923f8fdd0..0e147b6cac3 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -191,7 +191,7 @@ std::optional Offense::state_to_task(RobotIntent intent) { // Only get a new target position if we have reached our target if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { - Seeking seeker{robot_id_}; + Seeker seeker{robot_id_}; return seeker.get_task(intent, last_world_state_, this->field_dimensions_); } } diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index adc5d8ec3a3..75dc60aabc1 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -14,7 +14,7 @@ #include "rj_common/time.hpp" #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" -#include "seeking.hpp" +#include "seeker.hpp" namespace strategy { diff --git a/soccer/src/soccer/strategy/agent/position/seeking.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp similarity index 85% rename from soccer/src/soccer/strategy/agent/position/seeking.cpp rename to soccer/src/soccer/strategy/agent/position/seeker.cpp index 946019697f1..d3e41efacee 100644 --- a/soccer/src/soccer/strategy/agent/position/seeking.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -1,38 +1,38 @@ -#include "seeking.hpp" +#include "seeker.hpp" namespace strategy { -Seeking::Seeking(int robot_id) { robot_id_ = robot_id; } +Seeker::Seeker(int robot_id) { robot_id_ = robot_id; } -std::optional Seeking::get_task(RobotIntent intent, const WorldState* last_world_state, +std::optional Seeker::get_task(RobotIntent intent, const WorldState* last_world_state, FieldDimensions field_dimensions) { // Determine target position for seeking rj_geometry::Point current_loc = last_world_state->get_robot(true, robot_id_).pose.position(); - target_pt = get_open_point(last_world_state, current_loc, field_dimensions); + target_pt_ = get_open_point(last_world_state, current_loc, field_dimensions); planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; - planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; + planning::LinearMotionInstant goal{target_pt_, rj_geometry::Point{0.0, 0.0}}; intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; return intent; } -rj_geometry::Point Seeking::get_open_point(const WorldState* world_state, +rj_geometry::Point Seeker::get_open_point(const WorldState* world_state, rj_geometry::Point current_loc, - FieldDimensions field_dimensions) { - return Seeking::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); + const FieldDimensions& field_dimensions) const { + return Seeker::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); } -rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min_prec, +rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_prec, rj_geometry::Point current_point, const WorldState* world_state, - FieldDimensions field_dimensions) { + const FieldDimensions& field_dimensions) const { while (current_prec > min_prec) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point min = current_point; - double min_val = eval_point(ball_pos, current_point, world_state); + double min_val = Seeker::eval_point(ball_pos, current_point, world_state); double curr_val{}; // Points in a current_prec radius of the current point, at 45 degree intervals std::vector check_points{ @@ -55,7 +55,7 @@ rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min // Finds the best point out of the ones checked for (auto point : check_points) { - curr_val = eval_point(ball_pos, point, world_state); + curr_val = Seeker::eval_point(ball_pos, point, world_state); if (curr_val < min_val) { min_val = curr_val; min = point; @@ -67,7 +67,7 @@ rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min return current_point; } -rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { +rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, const FieldDimensions& field_dimensions) const { double BORDER_BUFFER = .2; double x = p.x(); double y = p.y(); @@ -128,7 +128,7 @@ rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions return rj_geometry::Point(x, y); } -double Seeking::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, +double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state) { // Determines 'how good' a point is // A higher value is a worse point diff --git a/soccer/src/soccer/strategy/agent/position/seeking.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp similarity index 79% rename from soccer/src/soccer/strategy/agent/position/seeking.hpp rename to soccer/src/soccer/strategy/agent/position/seeker.hpp index 19d5be5f088..381454e5ddc 100644 --- a/soccer/src/soccer/strategy/agent/position/seeking.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -20,13 +20,17 @@ namespace strategy { /* - * The Seeking role provides the implementation for a offensive robot that + * The Seeker role provides the implementation for a offensive robot that * is trying to get open, so that they can receive a pass */ -class Seeking : public RoleInterface { +class Seeker : public RoleInterface { public: - Seeking(int robot_id); - ~Seeking() = default; + Seeker(int robot_id); + ~Seeker() = default; + Seeker(const Seeker& other) = default; + Seeker(Seeker&& other) = default; + Seeker& operator=(const Seeker& other) = default; + Seeker& operator=(Seeker&& other) = default; /** * @brief Returns a seeker behavior which aims to get open @@ -44,7 +48,7 @@ class Seeking : public RoleInterface { // The seeker's id int robot_id_; // The taret point to move to - rj_geometry::Point target_pt{0.0, 0.0}; + rj_geometry::Point target_pt_{0.0, 0.0}; /** * @brief Returns the point which is most 'open' @@ -57,7 +61,7 @@ class Seeking : public RoleInterface { */ rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, - FieldDimensions field_dimensions); + const FieldDimensions& field_dimensions) const; /** * @brief Calculates which point is the best by iteratively searching around the robot @@ -73,7 +77,7 @@ class Seeking : public RoleInterface { rj_geometry::Point calculate_open_point(double current_prec, double min_prec, rj_geometry::Point current_point, const WorldState* world_state, - FieldDimensions field_dimensions); + const FieldDimensions& field_dimensions) const; /** * @brief Corrects the point to be within the field @@ -83,7 +87,7 @@ class Seeking : public RoleInterface { * * @return rj_geometry::Point The corrected point */ - rj_geometry::Point correct_point(rj_geometry::Point point, FieldDimensions field_dimensions); + rj_geometry::Point correct_point(rj_geometry::Point point, const FieldDimensions& field_dimensions) const; /** * @brief Calculates how 'good' a target point is @@ -94,7 +98,7 @@ class Seeking : public RoleInterface { * * @return double The evaluation of that target point */ - double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + static double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state); }; From c3a91f674d4bca341c9a45cefc4d910b238cd919 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 16 Jan 2024 21:30:12 -0500 Subject: [PATCH 09/22] Fixed heuristics for seeking --- .../strategy/agent/position/offense.cpp | 231 +++++++++++++++++- .../strategy/agent/position/offense.hpp | 20 ++ .../strategy/agent/position/position.cpp | 24 +- .../strategy/agent/position/position.hpp | 2 + 4 files changed, 267 insertions(+), 10 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 812f01bdb83..2d4494a634f 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -29,8 +29,12 @@ 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(); + if (robot_id_ == 4) { + next_state = AWAITING_SEND_PASS; + } else { + next_state = SEEKING; + } } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -58,7 +62,7 @@ Offense::State Offense::update_state() { } } else if (current_state_ == RECEIVING) { // transition to idling if we are close enough to the ball - if (distance_to_ball < ball_receive_distance_) { + if (distance_to_ball < 2*ball_receive_distance_) { next_state = IDLING; } } else if (current_state_ == STEALING) { @@ -77,7 +81,20 @@ Offense::State Offense::update_state() { if (check_is_done()) { next_state = IDLING; } + } else if (current_state_ == AWAITING_SEND_PASS) { + auto cur = std::chrono::high_resolution_clock::now(); + auto diff = cur - start; + if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2){ + Position::broadcast_direct_pass_request(); + start = std::chrono::high_resolution_clock::now(); + } + } else if (current_state_ == SEEKING) { + // if the ball comes close to it while it's trying to seek, it should no longer be trying to seek + if (distance_to_ball < ball_receive_distance_) { + // next_state = IDLING; + } } + return next_state; } @@ -94,7 +111,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,12 +190,195 @@ 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; + } else if (current_state_ == SEEKING) { + rj_geometry::Point current_loc = last_world_state_->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point curr_target = intent.motion_command.target.position; + if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { + target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); + } + planning::PathTargetFaceOption face_option = planning::FaceBall{}; + bool ignore_ball = false; + // SPDLOG_INFO("Robot ID: {}, Target Pt: ({}, {})", robot_id_, target_pt.x(), target_pt.y()); + planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; + intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + return intent; } // should be impossible to reach, but this is an EmptyMotionCommand return std::nullopt; } +rj_geometry::Point Offense::get_open_point(const WorldState* world_state, + rj_geometry::Point current_loc, + FieldDimensions field_dimensions) { + return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); +} + +rj_geometry::Point Offense::calculate_open_point(double current_prec, double min_prec, + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions) { + while (current_prec > min_prec) { + rj_geometry::Point ball_pos = world_state->ball.position; + rj_geometry::Point min = current_point; + double min_val = max_los(ball_pos, current_point, world_state); + double curr_val{}; + // Points in a current_prec radius of the current point, at 45 degree intervals + std::vector check_points{ + correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, + field_dimensions)}; + + for (auto point : check_points) { + curr_val = max_los(ball_pos, point, world_state); + if (curr_val < min_val) { + min_val = curr_val; + min = point; + } + SPDLOG_INFO("I am {}, possible pt ({}, {}), val {}", robot_id_, point.x(), point.y(), curr_val); + } + current_prec *= 0.5; + current_point = min; + } + return current_point; +} + +rj_geometry::Point Offense::random_noise(double prec) { + double x = (double)rand() / RAND_MAX * prec; + double y = (double)rand() / RAND_MAX * prec; + return rj_geometry::Point{x, y}; +} + +rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { + double BORDER_BUFFER = .2; + double x = p.x(); + double y = p.y(); + + // X Border + if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { + x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; + } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { + x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; + } + + // Y Border + if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { + y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; + } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { + y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; + } + + // Goalie Boxes + if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { + if (y > 4.5) { + y = 8.0 - BORDER_BUFFER; + } else { + y = 1.0 + BORDER_BUFFER; + } + + if (x > .5) { + x = 1.0 + BORDER_BUFFER; + } else { + x = -1.0 - BORDER_BUFFER; + } + } + + // Assigns robots to horizontal thirds + if (robot_id_ == 1) { + // Assign left + if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - + BORDER_BUFFER; + } + } else if (robot_id_ == 2) { + // Assign right + if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + + BORDER_BUFFER; + } + } else { + // Assign middle + if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + + BORDER_BUFFER; + } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - + BORDER_BUFFER; + } + } + + return rj_geometry::Point(x, y); +} + +double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state) { + + + rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; + if (goal_box.contains_point(current_point)) { + return 10000000; + } + + double max = 0; + double curr_dp; + for (auto robot : world_state->their_robots) { + curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + rj_geometry::Segment pass_path{ball_pos, current_point}; + double min_robot_dist = 10000; + float min_path_dist = 10000; + for (auto bot : world_state->their_robots) { + rj_geometry::Point opp_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); + } + + for (auto bot : world_state->our_robots) { + rj_geometry::Point ally_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); + } + + min_path_dist = 0.1 / min_path_dist; + min_robot_dist = 0.1 / min_robot_dist; + + for (auto robot : world_state->our_robots) { + curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + // Additional heuristics for calculating optimal point + double ball_proximity_loss = (current_point - ball_pos).mag() * .002; + double goal_distance_loss = (9.0 - current_point.y()) * .008; + + return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; +} + void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) { Position::receive_communication_response(response); @@ -205,6 +404,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 = 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; diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index e09b513f192..5c7b0d7fe6c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -34,6 +35,7 @@ class Offense : public Position { void derived_acknowledge_ball_in_transit() override; private: + rj_geometry::Point target_pt{0.0, 0.0}; bool kicking_{true}; std::optional derived_get_task(RobotIntent intent) override; @@ -49,6 +51,8 @@ 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 + SEEKING, // is trying to get open }; State update_state(); @@ -61,6 +65,21 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; + rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, + FieldDimensions field_dimensions); + + rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions); + rj_geometry::Point random_noise(double prec); + rj_geometry::Point calculate_open_point(double, double, rj_geometry::Point, + const WorldState* world_state, + FieldDimensions field_dimensions); + double max_los(rj_geometry::Point, rj_geometry::Point, const WorldState* world_state); + + int seeker_pos_; + std::string offense_type_; + + std::chrono::time_point start = std::chrono::high_resolution_clock::now(); + /** * @brief Send request to the other robots to see if this robot should be the scorer * @@ -77,6 +96,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 412990db21026aeff87db6624f53fed6582437db Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 16 Jan 2024 21:31:17 -0500 Subject: [PATCH 10/22] Removed log statement --- soccer/src/soccer/strategy/agent/position/offense.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 2d4494a634f..3140c2ee52a 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -252,7 +252,6 @@ rj_geometry::Point Offense::calculate_open_point(double current_prec, double min min_val = curr_val; min = point; } - SPDLOG_INFO("I am {}, possible pt ({}, {}), val {}", robot_id_, point.x(), point.y(), curr_val); } current_prec *= 0.5; current_point = min; From bd0f241855af8764c2a79d27a587a3a0a2a29b9d Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 21 Jan 2024 19:23:44 -0500 Subject: [PATCH 11/22] Added comments to offense.cpp --- .../src/soccer/strategy/agent/position/offense.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3140c2ee52a..09633be46d0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -195,14 +195,15 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { + //Determine target position for seeking rj_geometry::Point current_loc = last_world_state_->get_robot(true, robot_id_).pose.position(); rj_geometry::Point curr_target = intent.motion_command.target.position; + //Only look for a new position if we are done with current movement if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); } planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; - // SPDLOG_INFO("Robot ID: {}, Target Pt: ({}, {})", robot_id_, target_pt.x(), target_pt.y()); planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; return intent; @@ -328,13 +329,16 @@ rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state) { + //Determines 'how good' a point is + //A higher value is a worse point - + //Does not go into the goalie boxes rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; if (goal_box.contains_point(current_point)) { return 10000000; } + //Line of Sight Heuristic double max = 0; double curr_dp; for (auto robot : world_state->their_robots) { @@ -345,6 +349,8 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ } } + //Whether the path from ball to the point is blocked + //Same logic in passing to check if target is open rj_geometry::Segment pass_path{ball_pos, current_point}; double min_robot_dist = 10000; float min_path_dist = 10000; @@ -363,6 +369,7 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ min_path_dist = 0.1 / min_path_dist; min_robot_dist = 0.1 / min_robot_dist; + //More Line of Sight Heuristics for (auto robot : world_state->our_robots) { curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; @@ -375,6 +382,7 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ double ball_proximity_loss = (current_point - ball_pos).mag() * .002; double goal_distance_loss = (9.0 - current_point.y()) * .008; + //Final evaluation return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; } From fedf92c82598afb5a1ee93e2080f1be0a92abc3e Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 21 Jan 2024 19:26:57 -0500 Subject: [PATCH 12/22] Remove testing code --- soccer/src/soccer/strategy/agent/position/offense.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 09633be46d0..3581999213c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -30,11 +30,7 @@ Offense::State Offense::update_state() { if (current_state_ == IDLING) { // send_scorer_request(); - if (robot_id_ == 4) { - next_state = AWAITING_SEND_PASS; - } else { - next_state = SEEKING; - } + next_state = SEEKING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -82,6 +78,7 @@ Offense::State Offense::update_state() { next_state = IDLING; } } else if (current_state_ == AWAITING_SEND_PASS) { + //Only sends a pass request every 0.2 seconds auto cur = std::chrono::high_resolution_clock::now(); auto diff = cur - start; if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2){ @@ -91,7 +88,7 @@ Offense::State Offense::update_state() { } else if (current_state_ == SEEKING) { // if the ball comes close to it while it's trying to seek, it should no longer be trying to seek if (distance_to_ball < ball_receive_distance_) { - // next_state = IDLING; + // next_state = RECEIVING; } } From 6bf581e16d2d83efd71c81d989fc219de5909a35 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:34:52 -0500 Subject: [PATCH 13/22] Fix Code Style On seeking-test (#2161) automated style fixes Co-authored-by: sanatd33 --- .../strategy/agent/position/offense.cpp | 71 ++++++++++--------- .../strategy/agent/position/offense.hpp | 33 ++++----- .../strategy/agent/position/position.cpp | 2 +- 3 files changed, 56 insertions(+), 50 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 3581999213c..534e9a64cfb 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -58,7 +58,7 @@ Offense::State Offense::update_state() { } } else if (current_state_ == RECEIVING) { // transition to idling if we are close enough to the ball - if (distance_to_ball < 2*ball_receive_distance_) { + if (distance_to_ball < 2 * ball_receive_distance_) { next_state = IDLING; } } else if (current_state_ == STEALING) { @@ -78,20 +78,20 @@ Offense::State Offense::update_state() { next_state = IDLING; } } else if (current_state_ == AWAITING_SEND_PASS) { - //Only sends a pass request every 0.2 seconds + // Only sends a pass request every 0.2 seconds auto cur = std::chrono::high_resolution_clock::now(); auto diff = cur - start; - if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2){ + if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2) { Position::broadcast_direct_pass_request(); start = std::chrono::high_resolution_clock::now(); } - } else if (current_state_ == SEEKING) { - // if the ball comes close to it while it's trying to seek, it should no longer be trying to seek + } else if (current_state_ == SEEKING) { + // if the ball comes close to it while it's trying to seek, it should no longer be trying to + // seek if (distance_to_ball < ball_receive_distance_) { // next_state = RECEIVING; } } - return next_state; } @@ -192,17 +192,19 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { - //Determine target position for seeking - rj_geometry::Point current_loc = last_world_state_->get_robot(true, robot_id_).pose.position(); + // Determine target position for seeking + rj_geometry::Point current_loc = + last_world_state_->get_robot(true, robot_id_).pose.position(); rj_geometry::Point curr_target = intent.motion_command.target.position; - //Only look for a new position if we are done with current movement + // Only look for a new position if we are done with current movement if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); } planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + intent.motion_command = + planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; return intent; } @@ -211,15 +213,15 @@ std::optional Offense::state_to_task(RobotIntent intent) { } rj_geometry::Point Offense::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, - FieldDimensions field_dimensions) { + rj_geometry::Point current_loc, + FieldDimensions field_dimensions) { return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); } rj_geometry::Point Offense::calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - FieldDimensions field_dimensions) { + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions) { while (current_prec > min_prec) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point min = current_point; @@ -325,17 +327,17 @@ rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions } double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { - //Determines 'how good' a point is - //A higher value is a worse point + const WorldState* world_state) { + // Determines 'how good' a point is + // A higher value is a worse point - //Does not go into the goalie boxes + // Does not go into the goalie boxes rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; if (goal_box.contains_point(current_point)) { return 10000000; } - //Line of Sight Heuristic + // Line of Sight Heuristic double max = 0; double curr_dp; for (auto robot : world_state->their_robots) { @@ -346,8 +348,8 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ } } - //Whether the path from ball to the point is blocked - //Same logic in passing to check if target is open + // Whether the path from ball to the point is blocked + // Same logic in passing to check if target is open rj_geometry::Segment pass_path{ball_pos, current_point}; double min_robot_dist = 10000; float min_path_dist = 10000; @@ -362,11 +364,11 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); } - + min_path_dist = 0.1 / min_path_dist; min_robot_dist = 0.1 / min_robot_dist; - //More Line of Sight Heuristics + // More Line of Sight Heuristics for (auto robot : world_state->our_robots) { curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; @@ -379,7 +381,7 @@ double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_ double ball_proximity_loss = (current_point - ball_pos).mag() * .002; double goal_distance_loss = (9.0 - current_point.y()) * .008; - //Final evaluation + // Final evaluation return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; } @@ -410,24 +412,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 5c7b0d7fe6c..9202b71c398 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include +#include #include #include @@ -42,17 +42,17 @@ 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 - SEEKING, // is trying to get open + 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 + SEEKING, // is trying to get open }; State update_state(); @@ -65,8 +65,9 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; - rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, - FieldDimensions field_dimensions); + rj_geometry::Point get_open_point(const WorldState* world_state, + rj_geometry::Point current_position, + FieldDimensions field_dimensions); rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions); rj_geometry::Point random_noise(double prec); @@ -78,7 +79,8 @@ class Offense : public Position { int seeker_pos_; std::string offense_type_; - std::chrono::time_point start = std::chrono::high_resolution_clock::now(); + std::chrono::time_point start = + std::chrono::high_resolution_clock::now(); /** * @brief Send request to the other robots to see if this robot should be the scorer @@ -96,7 +98,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; From ccc56fceab107e4a1ae8ae6c50289ea89539a838 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Tue, 23 Jan 2024 20:06:27 -0500 Subject: [PATCH 14/22] Created Role Interface for Seeking --- soccer/src/soccer/CMakeLists.txt | 1 + .../strategy/agent/position/offense.cpp | 190 +---------------- .../strategy/agent/position/offense.hpp | 14 +- .../strategy/agent/position/seeking.cpp | 195 ++++++++++++++++++ .../strategy/agent/position/seeking.hpp | 100 +++++++++ 5 files changed, 302 insertions(+), 198 deletions(-) create mode 100644 soccer/src/soccer/strategy/agent/position/seeking.cpp create mode 100644 soccer/src/soccer/strategy/agent/position/seeking.hpp diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 9f93219dc06..9eb09b970fe 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -81,6 +81,7 @@ set(ROBOCUP_LIB_SRC strategy/agent/position/offense.cpp strategy/agent/position/defense.cpp strategy/agent/position/waller.cpp + strategy/agent/position/seeking.cpp strategy/agent/position/goal_kicker.cpp strategy/agent/position/penalty_player.cpp ) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 534e9a64cfb..7609cce0771 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -192,199 +192,17 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == SEEKING) { - // Determine target position for seeking - rj_geometry::Point current_loc = - last_world_state_->get_robot(true, robot_id_).pose.position(); - rj_geometry::Point curr_target = intent.motion_command.target.position; - // Only look for a new position if we are done with current movement - if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) { - target_pt = get_open_point(last_world_state_, current_loc, field_dimensions_); + //Only get a new target position if we have reached our target + if (check_is_done() || last_world_state_->get_robot(true, robot_id_).velocity.linear().mag() <= 0.01) { + Seeking seeker{robot_id_}; + return seeker.get_task(intent, last_world_state_, this->field_dimensions_); } - planning::PathTargetFaceOption face_option = planning::FaceBall{}; - bool ignore_ball = false; - planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = - planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; - return intent; } // should be impossible to reach, but this is an EmptyMotionCommand return std::nullopt; } -rj_geometry::Point Offense::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, - FieldDimensions field_dimensions) { - return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); -} - -rj_geometry::Point Offense::calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - FieldDimensions field_dimensions) { - while (current_prec > min_prec) { - rj_geometry::Point ball_pos = world_state->ball.position; - rj_geometry::Point min = current_point; - double min_val = max_los(ball_pos, current_point, world_state); - double curr_val{}; - // Points in a current_prec radius of the current point, at 45 degree intervals - std::vector check_points{ - correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), - correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), - correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), - correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), - correct_point( - current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, - field_dimensions)}; - - for (auto point : check_points) { - curr_val = max_los(ball_pos, point, world_state); - if (curr_val < min_val) { - min_val = curr_val; - min = point; - } - } - current_prec *= 0.5; - current_point = min; - } - return current_point; -} - -rj_geometry::Point Offense::random_noise(double prec) { - double x = (double)rand() / RAND_MAX * prec; - double y = (double)rand() / RAND_MAX * prec; - return rj_geometry::Point{x, y}; -} - -rj_geometry::Point Offense::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { - double BORDER_BUFFER = .2; - double x = p.x(); - double y = p.y(); - - // X Border - if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { - x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; - } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { - x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; - } - - // Y Border - if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { - y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; - } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { - y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; - } - - // Goalie Boxes - if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { - if (y > 4.5) { - y = 8.0 - BORDER_BUFFER; - } else { - y = 1.0 + BORDER_BUFFER; - } - - if (x > .5) { - x = 1.0 + BORDER_BUFFER; - } else { - x = -1.0 - BORDER_BUFFER; - } - } - - // Assigns robots to horizontal thirds - if (robot_id_ == 1) { - // Assign left - if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { - x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - - BORDER_BUFFER; - } - } else if (robot_id_ == 2) { - // Assign right - if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { - x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + - BORDER_BUFFER; - } - } else { - // Assign middle - if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { - x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + - BORDER_BUFFER; - } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { - x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - - BORDER_BUFFER; - } - } - - return rj_geometry::Point(x, y); -} - -double Offense::max_los(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { - // Determines 'how good' a point is - // A higher value is a worse point - - // Does not go into the goalie boxes - rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; - if (goal_box.contains_point(current_point)) { - return 10000000; - } - - // Line of Sight Heuristic - double max = 0; - double curr_dp; - for (auto robot : world_state->their_robots) { - curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); - curr_dp *= curr_dp; - if (curr_dp > max) { - max = curr_dp; - } - } - - // Whether the path from ball to the point is blocked - // Same logic in passing to check if target is open - rj_geometry::Segment pass_path{ball_pos, current_point}; - double min_robot_dist = 10000; - float min_path_dist = 10000; - for (auto bot : world_state->their_robots) { - rj_geometry::Point opp_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); - } - - for (auto bot : world_state->our_robots) { - rj_geometry::Point ally_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); - } - - min_path_dist = 0.1 / min_path_dist; - min_robot_dist = 0.1 / min_robot_dist; - - // More Line of Sight Heuristics - for (auto robot : world_state->our_robots) { - curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); - curr_dp *= curr_dp; - if (curr_dp > max) { - max = curr_dp; - } - } - - // Additional heuristics for calculating optimal point - double ball_proximity_loss = (current_point - ball_pos).mag() * .002; - double goal_distance_loss = (9.0 - current_point.y()) * .008; - - // Final evaluation - return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; -} - void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) { Position::receive_communication_response(response); diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 9202b71c398..81ea59eede0 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -15,6 +15,8 @@ #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" +#include "seeking.hpp" + namespace strategy { /* @@ -35,7 +37,6 @@ class Offense : public Position { void derived_acknowledge_ball_in_transit() override; private: - rj_geometry::Point target_pt{0.0, 0.0}; bool kicking_{true}; std::optional derived_get_task(RobotIntent intent) override; @@ -65,17 +66,6 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; - rj_geometry::Point get_open_point(const WorldState* world_state, - rj_geometry::Point current_position, - FieldDimensions field_dimensions); - - rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions); - rj_geometry::Point random_noise(double prec); - rj_geometry::Point calculate_open_point(double, double, rj_geometry::Point, - const WorldState* world_state, - FieldDimensions field_dimensions); - double max_los(rj_geometry::Point, rj_geometry::Point, const WorldState* world_state); - int seeker_pos_; std::string offense_type_; diff --git a/soccer/src/soccer/strategy/agent/position/seeking.cpp b/soccer/src/soccer/strategy/agent/position/seeking.cpp new file mode 100644 index 00000000000..3b9a060b229 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/seeking.cpp @@ -0,0 +1,195 @@ +#include "seeking.hpp" + +namespace strategy { + +Seeking::Seeking(int robot_id) { + robot_id_ = robot_id; +} + +std::optional Seeking::get_task(RobotIntent intent, const WorldState* last_world_state, + FieldDimensions field_dimensions) { + // Determine target position for seeking + rj_geometry::Point current_loc = + last_world_state->get_robot(true, robot_id_).pose.position(); + + target_pt = get_open_point(last_world_state, current_loc, field_dimensions); + + planning::PathTargetFaceOption face_option = planning::FaceBall{}; + bool ignore_ball = false; + planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; + intent.motion_command = + planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + + return intent; +} + +rj_geometry::Point Seeking::get_open_point(const WorldState* world_state, + rj_geometry::Point current_loc, + FieldDimensions field_dimensions) { + return Seeking::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); +} + +rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min_prec, + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions) { + while (current_prec > min_prec) { + rj_geometry::Point ball_pos = world_state->ball.position; + rj_geometry::Point min = current_point; + double min_val = eval_point(ball_pos, current_point, world_state); + double curr_val{}; + // Points in a current_prec radius of the current point, at 45 degree intervals + std::vector check_points{ + correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), + correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, + field_dimensions), + correct_point( + current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, + field_dimensions)}; + + + // Finds the best point out of the ones checked + for (auto point : check_points) { + curr_val = eval_point(ball_pos, point, world_state); + if (curr_val < min_val) { + min_val = curr_val; + min = point; + } + } + current_prec *= 0.5; + current_point = min; + } + return current_point; +} + +rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { + double BORDER_BUFFER = .2; + double x = p.x(); + double y = p.y(); + + // X Border + if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { + x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; + } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { + x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; + } + + // Y Border + if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { + y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; + } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { + y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; + } + + // Goalie Boxes + if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { + if (y > 4.5) { + y = 8.0 - BORDER_BUFFER; + } else { + y = 1.0 + BORDER_BUFFER; + } + + if (x > .5) { + x = 1.0 + BORDER_BUFFER; + } else { + x = -1.0 - BORDER_BUFFER; + } + } + + // Assigns robots to horizontal thirds + if (robot_id_ == 1) { + // Assign left + if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - + BORDER_BUFFER; + } + } else if (robot_id_ == 2) { + // Assign right + if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + + BORDER_BUFFER; + } + } else { + // Assign middle + if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { + x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + + BORDER_BUFFER; + } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { + x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - + BORDER_BUFFER; + } + } + + return rj_geometry::Point(x, y); +} + +double Seeking::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state) { + // Determines 'how good' a point is + // A higher value is a worse point + + // Does not go into the goalie boxes + rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; + if (goal_box.contains_point(current_point)) { + return 10000000; + } + + // Line of Sight Heuristic + double max = 0; + double curr_dp; + for (auto robot : world_state->their_robots) { + curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + // Whether the path from ball to the point is blocked + // Same logic in passing to check if target is open + rj_geometry::Segment pass_path{ball_pos, current_point}; + double min_robot_dist = 10000; + float min_path_dist = 10000; + for (auto bot : world_state->their_robots) { + rj_geometry::Point opp_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); + } + + for (auto bot : world_state->our_robots) { + rj_geometry::Point ally_pos = bot.pose.position(); + min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); + min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); + } + + min_path_dist = 0.1 / min_path_dist; + min_robot_dist = 0.1 / min_robot_dist; + + // More Line of Sight Heuristics + for (auto robot : world_state->our_robots) { + curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); + curr_dp *= curr_dp; + if (curr_dp > max) { + max = curr_dp; + } + } + + // Additional heuristics for calculating optimal point + double ball_proximity_loss = (current_point - ball_pos).mag() * .002; + double goal_distance_loss = (9.0 - current_point.y()) * .008; + + // Final evaluation + return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; +} + +} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/seeking.hpp b/soccer/src/soccer/strategy/agent/position/seeking.hpp new file mode 100644 index 00000000000..7b90ef07b41 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/seeking.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include + +#include +#include +#include + +#include + +#include "planning/instant.hpp" +#include "position.hpp" +#include "rj_common/field_dimensions.hpp" +#include "rj_common/time.hpp" +#include "rj_constants/constants.hpp" +#include "rj_geometry/geometry_conversions.hpp" +#include "rj_geometry/point.hpp" +#include "role_interface.hpp" + +namespace strategy { + +/* + * The Seeking role provides the implementation for a offensive robot that + * is trying to get open, so that they can receive a pass + */ +class Seeking : public RoleInterface { +public: + Seeking(int robot_id); + ~Seeking() = default; + + /** + * @brief Returns a seeker behavior which aims to get open + * + * @param intent The RobotIntent to add the movement to + * @param world_state The current WorldState + * @param field_dimensions The dimensions of the field + * + * @return [RobotIntent with next target point for the robot] + */ + std::optional get_task(RobotIntent intent, const WorldState* world_state, + FieldDimensions field_dimensions) override; + +private: + // The seeker's id + int robot_id_; + // The taret point to move to + rj_geometry::Point target_pt{0.0, 0.0}; + + /** + * @brief Returns the point which is most 'open' + * + * @param world_state The current WorldState + * @param current_position The current position of the seeker + * @param field_dimensions The dimensions of the field + * + * @return rj_geometry::Point The target point + */ + rj_geometry::Point get_open_point(const WorldState* world_state, + rj_geometry::Point current_position, + FieldDimensions field_dimensions); + + /** + * @brief Calculates which point is the best by iteratively searching around the robot + * + * @param current_prec A double that represents how far away to look from the robot + * @param min_prec A double that represents the minimum distance to look from the robot + * @param current_point The robot's current position + * @param world_state The current WorldState + * @param field_dimensions The dimensions of the field + * + * @return rj_geometry::Point The best point found + */ + rj_geometry::Point calculate_open_point(double current_prec, double min_prec, + rj_geometry::Point current_point, + const WorldState* world_state, + FieldDimensions field_dimensions); + + /** + * @brief Corrects the point to be within the field + * + * @param point The point to correct + * @param field_dimensions The dimensions of the field + * + * @return rj_geometry::Point The corrected point + */ + rj_geometry::Point correct_point(rj_geometry::Point point, FieldDimensions field_dimensions); + + /** + * @brief Calculates how 'good' a target point is + * + * @param ball_pos The current position of the ball + * @param current_point The point that is being evaluated + * @param world_state The current world state + * + * @return double The evaluation of that target point + */ + double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state); +}; + +} // namespace strategy From 7d8bebe563461fdce63ebe396fa614c95548a45b Mon Sep 17 00:00:00 2001 From: jvogt23 <91633361+jvogt23@users.noreply.github.com> Date: Tue, 23 Jan 2024 12:09:12 -0500 Subject: [PATCH 15/22] Fixed on_actionResetField_triggered() method (#2157) Added hard-coded robot positions in main_window reset method --- soccer/src/soccer/ui/main_window.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/soccer/src/soccer/ui/main_window.cpp b/soccer/src/soccer/ui/main_window.cpp index 8f9c9603e14..fbc1e3cbd57 100644 --- a/soccer/src/soccer/ui/main_window.cpp +++ b/soccer/src/soccer/ui/main_window.cpp @@ -828,17 +828,23 @@ void MainWindow::on_actionStopBall_triggered() { } void MainWindow::on_actionResetField_triggered() { - const int NUM_COLS = 2; - const int ROBOTS_PER_COL = kRobotsPerTeam / NUM_COLS; + _ui.fieldView->set_robot_pose(rj_geometry::Pose(2, -1, 0), 0, false); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(2, -1, 0), 0, true); - for (size_t i = 0; i < kRobotsPerTeam; ++i) { - double x_pos = +2.5 - i / ROBOTS_PER_COL; - double y_pos = i % ROBOTS_PER_COL - ROBOTS_PER_COL / NUM_COLS; - auto pose = rj_geometry::Pose(x_pos, y_pos, 0); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(2, 0, 0), 1, false); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(2, 0, 0), 1, true); - _ui.fieldView->set_robot_pose(pose, i, false); - _ui.fieldView->set_robot_pose(pose, i, true); - } + _ui.fieldView->set_robot_pose(rj_geometry::Pose(2, 1, 0), 2, false); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(2, 1, 0), 2, true); + + _ui.fieldView->set_robot_pose(rj_geometry::Pose(3, -1, 0), 3, false); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(3, -1, 0), 3, true); + + _ui.fieldView->set_robot_pose(rj_geometry::Pose(3, 0, 0), 4, false); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(3, 0, 0), 4, true); + + _ui.fieldView->set_robot_pose(rj_geometry::Pose(3, 1, 0), 5, false); + _ui.fieldView->set_robot_pose(rj_geometry::Pose(3, 1, 0), 5, true); _ui.fieldView->set_ball_position(rj_geometry::Point(0.0, 0.0)); _ui.fieldView->set_ball_velocity(rj_geometry::Point(0.0, 0.0)); From 26c7f7b51cbed6b26e648ce63b224548a5a570f2 Mon Sep 17 00:00:00 2001 From: Peter Garud <79226125+petergarud@users.noreply.github.com> Date: Tue, 23 Jan 2024 12:09:50 -0500 Subject: [PATCH 16/22] added play state to plan request (#2151) * added play state to plan request * Fix style * Fix Code Style On add-play-state-to-plan-request (#2166) automated style fixes Co-authored-by: sid-parikh --------- Co-authored-by: Sid Parikh Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: sid-parikh --- .../soccer/planning/planner/plan_request.hpp | 14 +++++++---- .../src/soccer/planning/planner_for_robot.cpp | 1 + .../soccer/planning/tests/planner_test.cpp | 23 +++++++++++++++++++ 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index 532ab073f92..a96f207ed5f 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -8,6 +8,7 @@ #include #include +#include "../global_state.hpp" #include "context.hpp" #include "planning/dynamic_obstacle.hpp" #include "planning/instant.hpp" @@ -28,9 +29,9 @@ struct PlanRequest { PlanRequest(RobotInstant start, MotionCommand command, // NOLINT RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles, rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories, - unsigned shell_id, const WorldState* world_state, int8_t priority = 0, - rj_drawing::RosDebugDrawer* debug_drawer = nullptr, bool ball_sense = false, - float min_dist_from_ball = 0, float dribbler_speed = 0) + unsigned shell_id, const WorldState* world_state, PlayState play_state, + int8_t priority = 0, rj_drawing::RosDebugDrawer* debug_drawer = nullptr, + bool ball_sense = false, float min_dist_from_ball = 0, float dribbler_speed = 0) : start(start), motion_command(command), // NOLINT constraints(constraints), @@ -40,6 +41,7 @@ struct PlanRequest { shell_id(shell_id), priority(priority), world_state(world_state), + play_state(play_state), debug_drawer(debug_drawer), ball_sense(ball_sense), min_dist_from_ball(min_dist_from_ball), @@ -95,6 +97,11 @@ struct PlanRequest { */ int8_t priority; + /** + * the current PlayState + */ + PlayState play_state; + /** * Allows debug drawing in the world. If this is nullptr, no debug drawing * should be performed. @@ -103,7 +110,6 @@ struct PlanRequest { // Whether the robot has a ball bool ball_sense = false; - /** * How far away to stay from the ball, if the MotionCommand chooses to avoid the ball. */ diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index 18914483dc7..e96789561e1 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -205,6 +205,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) { robot_trajectories_, static_cast(robot_id_), world_state, + play_state, intent.priority, &debug_draw_, had_break_beam_, diff --git a/soccer/src/soccer/planning/tests/planner_test.cpp b/soccer/src/soccer/planning/tests/planner_test.cpp index dd0d14fd826..838e264b5ea 100644 --- a/soccer/src/soccer/planning/tests/planner_test.cpp +++ b/soccer/src/soccer/planning/tests/planner_test.cpp @@ -2,6 +2,7 @@ #include +#include "game_state.hpp" #include "planning/instant.hpp" #include "planning/planner/collect_path_planner.hpp" #include "planning/planner/motion_command.hpp" @@ -47,6 +48,7 @@ TEST(Planning, path_target_random) { } LinearMotionInstant goal = random_instant(&gen).linear_motion(); + PlayState play_state = PlayState::halt(); PlanRequest request{start, MotionCommand{"path_target", goal}, RobotConstraints{}, @@ -55,6 +57,7 @@ TEST(Planning, path_target_random) { {}, 0, &world_state, + play_state, 2, nullptr}; Trajectory path = planner.plan(std::move(request)); @@ -87,6 +90,7 @@ TEST(Planning, collect_basic) { world_state.ball.position = Point{1, 1}; world_state.ball.velocity = Point{0, 0}; world_state.ball.timestamp = RJ::now(); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -95,6 +99,7 @@ TEST(Planning, collect_basic) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -109,6 +114,7 @@ TEST(Planning, collect_obstructed) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{.5, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -117,6 +123,7 @@ TEST(Planning, collect_obstructed) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -134,6 +141,7 @@ TEST(Planning, collect_pointless_obs) { obstacles.add(std::make_shared(Point{3, 3}, .2)); obstacles.add(std::make_shared(Point{-2, 3}, .2)); obstacles.add(std::make_shared(Point{0, 5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -142,6 +150,7 @@ TEST(Planning, collect_pointless_obs) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -156,6 +165,7 @@ TEST(Planning, collect_moving_ball_quick) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{0, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -164,6 +174,7 @@ TEST(Planning, collect_moving_ball_quick) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -178,6 +189,7 @@ TEST(Planning, collect_moving_ball_slow) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{-0.5, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -186,6 +198,7 @@ TEST(Planning, collect_moving_ball_slow) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -200,6 +213,7 @@ TEST(Planning, collect_moving_ball_slow_2) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{0, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -208,6 +222,7 @@ TEST(Planning, collect_moving_ball_slow_2) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -234,6 +249,7 @@ TEST(Planning, collect_random) { Point{TestingUtils::random(&gen, -2.0, 2.0), TestingUtils::random(&gen, 0.5, 1.5)}, .2)); } + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"collect"}, RobotConstraints{}, @@ -242,6 +258,7 @@ TEST(Planning, collect_random) { {}, 0, &world_state, + play_state, 2, nullptr}; CollectPathPlanner planner; @@ -265,6 +282,7 @@ TEST(Planning, settle_basic) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{.5, .5}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"settle"}, RobotConstraints{}, @@ -273,6 +291,7 @@ TEST(Planning, settle_basic) { {}, 0, &world_state, + play_state, 2, nullptr}; SettlePathPlanner planner; @@ -289,6 +308,7 @@ TEST(Planning, settle_pointless_obs) { world_state.ball.timestamp = RJ::now(); ShapeSet obstacles; obstacles.add(std::make_shared(Point{-1, 1.0}, .2)); + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"settle"}, RobotConstraints{}, @@ -297,6 +317,7 @@ TEST(Planning, settle_pointless_obs) { {}, 0, &world_state, + play_state, 2, nullptr}; SettlePathPlanner planner; @@ -324,6 +345,7 @@ TEST(Planning, settle_random) { Point{TestingUtils::random(&gen, -2.0, 2.0), TestingUtils::random(&gen, .5, 1.5)}, .2)); } + PlayState play_state = PlayState::halt(); PlanRequest request{RobotInstant{{}, {}, RJ::now()}, MotionCommand{"settle"}, RobotConstraints{}, @@ -332,6 +354,7 @@ TEST(Planning, settle_random) { {}, 0, &world_state, + play_state, 2, nullptr}; SettlePathPlanner planner; From a7cc97bc1c47db24eba217061296a51868321d51 Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 28 Jan 2024 20:41:28 -0500 Subject: [PATCH 17/22] Fixed merge conflict --- .../src/soccer/strategy/agent/position/offense.cpp | 12 +----------- .../src/soccer/strategy/agent/position/offense.hpp | 6 ------ 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 7609cce0771..2672878e168 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -78,18 +78,8 @@ Offense::State Offense::update_state() { next_state = IDLING; } } else if (current_state_ == AWAITING_SEND_PASS) { - // Only sends a pass request every 0.2 seconds - auto cur = std::chrono::high_resolution_clock::now(); - auto diff = cur - start; - if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2) { + if (distance_to_ball < ball_lost_distance_) { Position::broadcast_direct_pass_request(); - start = std::chrono::high_resolution_clock::now(); - } - } else if (current_state_ == SEEKING) { - // if the ball comes close to it while it's trying to seek, it should no longer be trying to - // seek - if (distance_to_ball < ball_receive_distance_) { - // next_state = RECEIVING; } } diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 81ea59eede0..f5b99221b3a 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -66,12 +66,6 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; - int seeker_pos_; - std::string offense_type_; - - std::chrono::time_point start = - std::chrono::high_resolution_clock::now(); - /** * @brief Send request to the other robots to see if this robot should be the scorer * From ae1a663059eb032686a2401f6e32e465c95899f1 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 28 Jan 2024 20:49:31 -0500 Subject: [PATCH 18/22] Fix Code Style On seeking-test (#2175) automated style fixes Co-authored-by: sanatd33 --- .../strategy/agent/position/offense.hpp | 1 - .../soccer/strategy/agent/position/seeker.cpp | 17 ++++++----- .../soccer/strategy/agent/position/seeker.hpp | 5 ++-- .../strategy/agent/position/seeking.cpp | 17 ++++------- .../strategy/agent/position/seeking.hpp | 29 ++++++++++--------- 5 files changed, 33 insertions(+), 36 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index ade908b366b..cee1d2512bc 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -15,7 +15,6 @@ #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" #include "seeker.hpp" - #include "seeking.hpp" namespace strategy { diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index d3e41efacee..bca10e966b4 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -5,7 +5,7 @@ namespace strategy { Seeker::Seeker(int robot_id) { robot_id_ = robot_id; } std::optional Seeker::get_task(RobotIntent intent, const WorldState* last_world_state, - FieldDimensions field_dimensions) { + FieldDimensions field_dimensions) { // Determine target position for seeking rj_geometry::Point current_loc = last_world_state->get_robot(true, robot_id_).pose.position(); @@ -20,15 +20,15 @@ std::optional Seeker::get_task(RobotIntent intent, const WorldState } rj_geometry::Point Seeker::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, - const FieldDimensions& field_dimensions) const { + rj_geometry::Point current_loc, + const FieldDimensions& field_dimensions) const { return Seeker::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); } rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - const FieldDimensions& field_dimensions) const { + rj_geometry::Point current_point, + const WorldState* world_state, + const FieldDimensions& field_dimensions) const { while (current_prec > min_prec) { rj_geometry::Point ball_pos = world_state->ball.position; rj_geometry::Point min = current_point; @@ -67,7 +67,8 @@ rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_ return current_point; } -rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, const FieldDimensions& field_dimensions) const { +rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, + const FieldDimensions& field_dimensions) const { double BORDER_BUFFER = .2; double x = p.x(); double y = p.y(); @@ -129,7 +130,7 @@ rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, const FieldDimens } double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { + const WorldState* world_state) { // Determines 'how good' a point is // A higher value is a worse point diff --git a/soccer/src/soccer/strategy/agent/position/seeker.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp index 381454e5ddc..2b646309f98 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -87,7 +87,8 @@ class Seeker : public RoleInterface { * * @return rj_geometry::Point The corrected point */ - rj_geometry::Point correct_point(rj_geometry::Point point, const FieldDimensions& field_dimensions) const; + rj_geometry::Point correct_point(rj_geometry::Point point, + const FieldDimensions& field_dimensions) const; /** * @brief Calculates how 'good' a target point is @@ -99,7 +100,7 @@ class Seeker : public RoleInterface { * @return double The evaluation of that target point */ static double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state); + const WorldState* world_state); }; } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/seeking.cpp b/soccer/src/soccer/strategy/agent/position/seeking.cpp index 3b9a060b229..946019697f1 100644 --- a/soccer/src/soccer/strategy/agent/position/seeking.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeking.cpp @@ -2,24 +2,20 @@ namespace strategy { -Seeking::Seeking(int robot_id) { - robot_id_ = robot_id; -} +Seeking::Seeking(int robot_id) { robot_id_ = robot_id; } std::optional Seeking::get_task(RobotIntent intent, const WorldState* last_world_state, - FieldDimensions field_dimensions) { + FieldDimensions field_dimensions) { // Determine target position for seeking - rj_geometry::Point current_loc = - last_world_state->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point current_loc = last_world_state->get_robot(true, robot_id_).pose.position(); target_pt = get_open_point(last_world_state, current_loc, field_dimensions); planning::PathTargetFaceOption face_option = planning::FaceBall{}; bool ignore_ball = false; planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = - planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; - + intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + return intent; } @@ -57,7 +53,6 @@ rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, field_dimensions)}; - // Finds the best point out of the ones checked for (auto point : check_points) { curr_val = eval_point(ball_pos, point, world_state); @@ -134,7 +129,7 @@ rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions } double Seeking::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { + const WorldState* world_state) { // Determines 'how good' a point is // A higher value is a worse point diff --git a/soccer/src/soccer/strategy/agent/position/seeking.hpp b/soccer/src/soccer/strategy/agent/position/seeking.hpp index 7b90ef07b41..19d5be5f088 100644 --- a/soccer/src/soccer/strategy/agent/position/seeking.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeking.hpp @@ -34,7 +34,7 @@ class Seeking : public RoleInterface { * @param intent The RobotIntent to add the movement to * @param world_state The current WorldState * @param field_dimensions The dimensions of the field - * + * * @return [RobotIntent with next target point for the robot] */ std::optional get_task(RobotIntent intent, const WorldState* world_state, @@ -48,28 +48,28 @@ class Seeking : public RoleInterface { /** * @brief Returns the point which is most 'open' - * + * * @param world_state The current WorldState * @param current_position The current position of the seeker * @param field_dimensions The dimensions of the field - * + * * @return rj_geometry::Point The target point - */ + */ rj_geometry::Point get_open_point(const WorldState* world_state, rj_geometry::Point current_position, FieldDimensions field_dimensions); /** * @brief Calculates which point is the best by iteratively searching around the robot - * + * * @param current_prec A double that represents how far away to look from the robot * @param min_prec A double that represents the minimum distance to look from the robot * @param current_point The robot's current position * @param world_state The current WorldState * @param field_dimensions The dimensions of the field - * + * * @return rj_geometry::Point The best point found - */ + */ rj_geometry::Point calculate_open_point(double current_prec, double min_prec, rj_geometry::Point current_point, const WorldState* world_state, @@ -77,24 +77,25 @@ class Seeking : public RoleInterface { /** * @brief Corrects the point to be within the field - * + * * @param point The point to correct * @param field_dimensions The dimensions of the field - * + * * @return rj_geometry::Point The corrected point - */ + */ rj_geometry::Point correct_point(rj_geometry::Point point, FieldDimensions field_dimensions); /** * @brief Calculates how 'good' a target point is - * + * * @param ball_pos The current position of the ball * @param current_point The point that is being evaluated * @param world_state The current world state - * + * * @return double The evaluation of that target point - */ - double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, const WorldState* world_state); + */ + double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state); }; } // namespace strategy From d7279696a22b0b2f3bfec2830ddbd9df8473850e Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Sun, 28 Jan 2024 20:50:55 -0500 Subject: [PATCH 19/22] Removed previous include --- soccer/src/soccer/strategy/agent/position/offense.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index cee1d2512bc..75dc60aabc1 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -15,7 +15,6 @@ #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" #include "seeker.hpp" -#include "seeking.hpp" namespace strategy { From 8d7e5f64c9584aa80b14acb2b755a46af3e0bf0c Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Mon, 29 Jan 2024 18:24:11 -0500 Subject: [PATCH 20/22] Resolved Warnings --- .../soccer/strategy/agent/position/seeker.cpp | 42 ++-- .../soccer/strategy/agent/position/seeker.hpp | 2 +- .../strategy/agent/position/seeking.cpp | 190 ------------------ .../strategy/agent/position/seeking.hpp | 101 ---------- 4 files changed, 22 insertions(+), 313 deletions(-) delete mode 100644 soccer/src/soccer/strategy/agent/position/seeking.cpp delete mode 100644 soccer/src/soccer/strategy/agent/position/seeking.hpp diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index bca10e966b4..1f2e2752c63 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -20,9 +20,9 @@ std::optional Seeker::get_task(RobotIntent intent, const WorldState } rj_geometry::Point Seeker::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, + rj_geometry::Point current_position, const FieldDimensions& field_dimensions) const { - return Seeker::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); + return Seeker::calculate_open_point(3.0, .2, current_position, world_state, field_dimensions); } rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_prec, @@ -69,36 +69,36 @@ rj_geometry::Point Seeker::calculate_open_point(double current_prec, double min_ rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, const FieldDimensions& field_dimensions) const { - double BORDER_BUFFER = .2; + double border_buffer = .2; double x = p.x(); double y = p.y(); // X Border - if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { - x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; - } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { - x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; + if (p.x() > field_dimensions.field_x_right_coord() - border_buffer) { + x = field_dimensions.field_x_right_coord() - border_buffer; + } else if (p.x() < field_dimensions.field_x_left_coord() + border_buffer) { + x = field_dimensions.field_x_left_coord() + border_buffer; } // Y Border - if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { - y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; - } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { - y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; + if (p.y() > field_dimensions.their_goal_loc().y() - border_buffer) { + y = field_dimensions.their_goal_loc().y() - border_buffer; + } else if (p.y() < field_dimensions.our_goal_loc().y() + border_buffer) { + y = field_dimensions.our_goal_loc().y() + border_buffer; } // Goalie Boxes if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { if (y > 4.5) { - y = 8.0 - BORDER_BUFFER; + y = 8.0 - border_buffer; } else { - y = 1.0 + BORDER_BUFFER; + y = 1.0 + border_buffer; } if (x > .5) { - x = 1.0 + BORDER_BUFFER; + x = 1.0 + border_buffer; } else { - x = -1.0 - BORDER_BUFFER; + x = -1.0 - border_buffer; } } @@ -107,22 +107,22 @@ rj_geometry::Point Seeker::correct_point(rj_geometry::Point p, // Assign left if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - - BORDER_BUFFER; + border_buffer; } } else if (robot_id_ == 2) { // Assign right if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + - BORDER_BUFFER; + border_buffer; } } else { // Assign middle if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + - BORDER_BUFFER; + border_buffer; } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - - BORDER_BUFFER; + border_buffer; } } @@ -142,7 +142,7 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren // Line of Sight Heuristic double max = 0; - double curr_dp; + double curr_dp = 0; for (auto robot : world_state->their_robots) { curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); curr_dp *= curr_dp; @@ -169,7 +169,7 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren } min_path_dist = 0.1 / min_path_dist; - min_robot_dist = 0.1 / min_robot_dist; + min_robot_dist = 0.1f / min_robot_dist; // More Line of Sight Heuristics for (auto robot : world_state->our_robots) { diff --git a/soccer/src/soccer/strategy/agent/position/seeker.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp index 2b646309f98..e39aa5e0926 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -87,7 +87,7 @@ class Seeker : public RoleInterface { * * @return rj_geometry::Point The corrected point */ - rj_geometry::Point correct_point(rj_geometry::Point point, + [[nodiscard]] rj_geometry::Point correct_point(rj_geometry::Point point, const FieldDimensions& field_dimensions) const; /** diff --git a/soccer/src/soccer/strategy/agent/position/seeking.cpp b/soccer/src/soccer/strategy/agent/position/seeking.cpp deleted file mode 100644 index 946019697f1..00000000000 --- a/soccer/src/soccer/strategy/agent/position/seeking.cpp +++ /dev/null @@ -1,190 +0,0 @@ -#include "seeking.hpp" - -namespace strategy { - -Seeking::Seeking(int robot_id) { robot_id_ = robot_id; } - -std::optional Seeking::get_task(RobotIntent intent, const WorldState* last_world_state, - FieldDimensions field_dimensions) { - // Determine target position for seeking - rj_geometry::Point current_loc = last_world_state->get_robot(true, robot_id_).pose.position(); - - target_pt = get_open_point(last_world_state, current_loc, field_dimensions); - - planning::PathTargetFaceOption face_option = planning::FaceBall{}; - bool ignore_ball = false; - planning::LinearMotionInstant goal{target_pt, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; - - return intent; -} - -rj_geometry::Point Seeking::get_open_point(const WorldState* world_state, - rj_geometry::Point current_loc, - FieldDimensions field_dimensions) { - return Seeking::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions); -} - -rj_geometry::Point Seeking::calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - FieldDimensions field_dimensions) { - while (current_prec > min_prec) { - rj_geometry::Point ball_pos = world_state->ball.position; - rj_geometry::Point min = current_point; - double min_val = eval_point(ball_pos, current_point, world_state); - double curr_val{}; - // Points in a current_prec radius of the current point, at 45 degree intervals - std::vector check_points{ - correct_point(current_point + rj_geometry::Point{current_prec, 0}, field_dimensions), - correct_point(current_point + rj_geometry::Point{-current_prec, 0}, field_dimensions), - correct_point(current_point + rj_geometry::Point{0, current_prec}, field_dimensions), - correct_point(current_point + rj_geometry::Point{0, -current_prec}, field_dimensions), - correct_point( - current_point + rj_geometry::Point{current_prec * 0.707, current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{current_prec * 0.707, -current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{-current_prec * 0.707, current_prec * 0.707}, - field_dimensions), - correct_point( - current_point + rj_geometry::Point{-current_prec * 0.707, -current_prec * 0.707}, - field_dimensions)}; - - // Finds the best point out of the ones checked - for (auto point : check_points) { - curr_val = eval_point(ball_pos, point, world_state); - if (curr_val < min_val) { - min_val = curr_val; - min = point; - } - } - current_prec *= 0.5; - current_point = min; - } - return current_point; -} - -rj_geometry::Point Seeking::correct_point(rj_geometry::Point p, FieldDimensions field_dimensions) { - double BORDER_BUFFER = .2; - double x = p.x(); - double y = p.y(); - - // X Border - if (p.x() > field_dimensions.field_x_right_coord() - BORDER_BUFFER) { - x = field_dimensions.field_x_right_coord() - BORDER_BUFFER; - } else if (p.x() < field_dimensions.field_x_left_coord() + BORDER_BUFFER) { - x = field_dimensions.field_x_left_coord() + BORDER_BUFFER; - } - - // Y Border - if (p.y() > field_dimensions.their_goal_loc().y() - BORDER_BUFFER) { - y = field_dimensions.their_goal_loc().y() - BORDER_BUFFER; - } else if (p.y() < field_dimensions.our_goal_loc().y() + BORDER_BUFFER) { - y = field_dimensions.our_goal_loc().y() + BORDER_BUFFER; - } - - // Goalie Boxes - if ((y < 1.2 || y > 7.8) && fabs(x) < 1.2) { - if (y > 4.5) { - y = 8.0 - BORDER_BUFFER; - } else { - y = 1.0 + BORDER_BUFFER; - } - - if (x > .5) { - x = 1.0 + BORDER_BUFFER; - } else { - x = -1.0 - BORDER_BUFFER; - } - } - - // Assigns robots to horizontal thirds - if (robot_id_ == 1) { - // Assign left - if (x > field_dimensions.field_x_left_coord() + field_dimensions.width() / 2) { - x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 2 - - BORDER_BUFFER; - } - } else if (robot_id_ == 2) { - // Assign right - if (x < field_dimensions.field_x_right_coord() - field_dimensions.width() / 2) { - x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 2 + - BORDER_BUFFER; - } - } else { - // Assign middle - if (x < field_dimensions.field_x_left_coord() + field_dimensions.width() / 3) { - x = field_dimensions.field_x_left_coord() + field_dimensions.width() / 3 + - BORDER_BUFFER; - } else if (x > field_dimensions.field_x_right_coord() - field_dimensions.width() / 3) { - x = field_dimensions.field_x_right_coord() - field_dimensions.width() / 3 - - BORDER_BUFFER; - } - } - - return rj_geometry::Point(x, y); -} - -double Seeking::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state) { - // Determines 'how good' a point is - // A higher value is a worse point - - // Does not go into the goalie boxes - rj_geometry::Rect goal_box{rj_geometry::Point{1, 8}, rj_geometry::Point{-1, 9}}; - if (goal_box.contains_point(current_point)) { - return 10000000; - } - - // Line of Sight Heuristic - double max = 0; - double curr_dp; - for (auto robot : world_state->their_robots) { - curr_dp = (current_point).norm().dot((robot.pose.position() - ball_pos).norm()); - curr_dp *= curr_dp; - if (curr_dp > max) { - max = curr_dp; - } - } - - // Whether the path from ball to the point is blocked - // Same logic in passing to check if target is open - rj_geometry::Segment pass_path{ball_pos, current_point}; - double min_robot_dist = 10000; - float min_path_dist = 10000; - for (auto bot : world_state->their_robots) { - rj_geometry::Point opp_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(opp_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos)); - } - - for (auto bot : world_state->our_robots) { - rj_geometry::Point ally_pos = bot.pose.position(); - min_robot_dist = std::min(min_robot_dist, current_point.dist_to(ally_pos)); - min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); - } - - min_path_dist = 0.1 / min_path_dist; - min_robot_dist = 0.1 / min_robot_dist; - - // More Line of Sight Heuristics - for (auto robot : world_state->our_robots) { - curr_dp = (current_point - ball_pos).norm().dot((robot.pose.position() - ball_pos).norm()); - curr_dp *= curr_dp; - if (curr_dp > max) { - max = curr_dp; - } - } - - // Additional heuristics for calculating optimal point - double ball_proximity_loss = (current_point - ball_pos).mag() * .002; - double goal_distance_loss = (9.0 - current_point.y()) * .008; - - // Final evaluation - return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; -} - -} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/seeking.hpp b/soccer/src/soccer/strategy/agent/position/seeking.hpp deleted file mode 100644 index 19d5be5f088..00000000000 --- a/soccer/src/soccer/strategy/agent/position/seeking.hpp +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include - -#include "planning/instant.hpp" -#include "position.hpp" -#include "rj_common/field_dimensions.hpp" -#include "rj_common/time.hpp" -#include "rj_constants/constants.hpp" -#include "rj_geometry/geometry_conversions.hpp" -#include "rj_geometry/point.hpp" -#include "role_interface.hpp" - -namespace strategy { - -/* - * The Seeking role provides the implementation for a offensive robot that - * is trying to get open, so that they can receive a pass - */ -class Seeking : public RoleInterface { -public: - Seeking(int robot_id); - ~Seeking() = default; - - /** - * @brief Returns a seeker behavior which aims to get open - * - * @param intent The RobotIntent to add the movement to - * @param world_state The current WorldState - * @param field_dimensions The dimensions of the field - * - * @return [RobotIntent with next target point for the robot] - */ - std::optional get_task(RobotIntent intent, const WorldState* world_state, - FieldDimensions field_dimensions) override; - -private: - // The seeker's id - int robot_id_; - // The taret point to move to - rj_geometry::Point target_pt{0.0, 0.0}; - - /** - * @brief Returns the point which is most 'open' - * - * @param world_state The current WorldState - * @param current_position The current position of the seeker - * @param field_dimensions The dimensions of the field - * - * @return rj_geometry::Point The target point - */ - rj_geometry::Point get_open_point(const WorldState* world_state, - rj_geometry::Point current_position, - FieldDimensions field_dimensions); - - /** - * @brief Calculates which point is the best by iteratively searching around the robot - * - * @param current_prec A double that represents how far away to look from the robot - * @param min_prec A double that represents the minimum distance to look from the robot - * @param current_point The robot's current position - * @param world_state The current WorldState - * @param field_dimensions The dimensions of the field - * - * @return rj_geometry::Point The best point found - */ - rj_geometry::Point calculate_open_point(double current_prec, double min_prec, - rj_geometry::Point current_point, - const WorldState* world_state, - FieldDimensions field_dimensions); - - /** - * @brief Corrects the point to be within the field - * - * @param point The point to correct - * @param field_dimensions The dimensions of the field - * - * @return rj_geometry::Point The corrected point - */ - rj_geometry::Point correct_point(rj_geometry::Point point, FieldDimensions field_dimensions); - - /** - * @brief Calculates how 'good' a target point is - * - * @param ball_pos The current position of the ball - * @param current_point The point that is being evaluated - * @param world_state The current world state - * - * @return double The evaluation of that target point - */ - double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, - const WorldState* world_state); -}; - -} // namespace strategy From 40e36623193cac741b1fd22cff44cf4409a35789 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 29 Jan 2024 18:26:15 -0500 Subject: [PATCH 21/22] Fix Code Style On seeking-test (#2181) automated style fixes Co-authored-by: sanatd33 --- soccer/src/soccer/strategy/agent/position/seeker.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/seeker.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp index e39aa5e0926..e6296b296ae 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.hpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -88,7 +88,7 @@ class Seeker : public RoleInterface { * @return rj_geometry::Point The corrected point */ [[nodiscard]] rj_geometry::Point correct_point(rj_geometry::Point point, - const FieldDimensions& field_dimensions) const; + const FieldDimensions& field_dimensions) const; /** * @brief Calculates how 'good' a target point is From 007f0a170c686b02ca5eb3cc3f9d33818b68f570 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Mon, 29 Jan 2024 18:40:35 -0500 Subject: [PATCH 22/22] Resolved conversion from float to double --- soccer/src/soccer/strategy/agent/position/seeker.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp index 1f2e2752c63..65467d376a9 100644 --- a/soccer/src/soccer/strategy/agent/position/seeker.cpp +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -168,8 +168,8 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren min_path_dist = std::min(min_path_dist, pass_path.dist_to(ally_pos)); } - min_path_dist = 0.1 / min_path_dist; - min_robot_dist = 0.1f / min_robot_dist; + min_path_dist = 0.1f / min_path_dist; + min_robot_dist = 0.1 / min_robot_dist; // More Line of Sight Heuristics for (auto robot : world_state->our_robots) { @@ -188,4 +188,4 @@ double Seeker::eval_point(rj_geometry::Point ball_pos, rj_geometry::Point curren return max + ball_proximity_loss + goal_distance_loss + min_path_dist + min_robot_dist; } -} // namespace strategy \ No newline at end of file +} // namespace strategy