From 5ad7a290f6dd4bdeec1003456031b903302c0f4a Mon Sep 17 00:00:00 2001 From: sanatd33 Date: Sun, 28 Jan 2024 21:44:06 -0500 Subject: [PATCH 1/3] Added function to see if there is open shot --- .../strategy/agent/position/offense.cpp | 50 ++++++++++++++++++- .../strategy/agent/position/offense.hpp | 4 ++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 4cb3b327cc1..321f9b7f7db 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -94,7 +94,7 @@ Offense::State Offense::update_state() { std::optional Offense::state_to_task(RobotIntent intent) { float dist{0.0f}; - SPDLOG_INFO(current_state_); + // SPDLOG_INFO(current_state_); if (current_state_ == IDLING) { // Do nothing auto empty_motion_cmd = planning::MotionCommand{}; @@ -215,6 +215,54 @@ std::optional Offense::state_to_task(RobotIntent intent) { return std::nullopt; } +bool Offense::has_open_shot() { + + // Goal location + rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); + double goal_width = field_dimensions_.goal_width(); // 1.0 meters + + // Ball location + rj_geometry::Point ball_position = this->last_world_state_->ball.position; + + double best_distance = -1.0; + rj_geometry::Point increment(0.05, 0); + rj_geometry::Point curr_point = their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; + for (int i = 0; i < 19; i++) { + double distance = distance_from_their_robots(ball_position, curr_point); + if (distance > best_distance) { + best_distance = distance; + } + curr_point = curr_point + increment; + } + + return best_distance > max_receive_distance; +} + +double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) { + rj_geometry::Point vec = head - tail; + auto their_robots = this->last_world_state_->their_robots; + double min_angle = -0.5; + for (auto enemy : their_robots) { + rj_geometry::Point enemy_vec = enemy.pose.position() - tail; + if (enemy_vec.dot(vec) < 0 ) { + continue; + } + auto projection = (enemy_vec.dot(vec)/vec.dot(vec)); + enemy_vec = enemy_vec - (projection) * vec; + double distance = enemy_vec.mag(); + if (distance < (kRobotRadius + kBallRadius)) { + return -1.0; + } + double angle = distance / projection; + if (min_angle < 0) { + min_angle = angle; + } else if(angle < min_angle) { + min_angle = angle; + } + } + return min_angle; +} + 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 4b302457372..66b60bea296 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -69,6 +69,10 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; + bool has_open_shot(); + + double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head); + /** * @brief Send request to the other robots to see if this robot should be the scorer * From e8c8003a5289e3c7ffdd0b1e6550cbbce88d58e6 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 28 Jan 2024 22:12:42 -0500 Subject: [PATCH 2/3] Fix Code Style On has-open-shot (#2179) automated style fixes Co-authored-by: sanatd33 --- .../src/soccer/strategy/agent/position/offense.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 321f9b7f7db..b0e75a79707 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -216,17 +216,17 @@ std::optional Offense::state_to_task(RobotIntent intent) { } bool Offense::has_open_shot() { - // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - double goal_width = field_dimensions_.goal_width(); // 1.0 meters + double goal_width = field_dimensions_.goal_width(); // 1.0 meters // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; double best_distance = -1.0; rj_geometry::Point increment(0.05, 0); - rj_geometry::Point curr_point = their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; + rj_geometry::Point curr_point = + their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; for (int i = 0; i < 19; i++) { double distance = distance_from_their_robots(ball_position, curr_point); if (distance > best_distance) { @@ -244,11 +244,11 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: double min_angle = -0.5; for (auto enemy : their_robots) { rj_geometry::Point enemy_vec = enemy.pose.position() - tail; - if (enemy_vec.dot(vec) < 0 ) { + if (enemy_vec.dot(vec) < 0) { continue; } - auto projection = (enemy_vec.dot(vec)/vec.dot(vec)); - enemy_vec = enemy_vec - (projection) * vec; + auto projection = (enemy_vec.dot(vec) / vec.dot(vec)); + enemy_vec = enemy_vec - (projection)*vec; double distance = enemy_vec.mag(); if (distance < (kRobotRadius + kBallRadius)) { return -1.0; @@ -256,7 +256,7 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: double angle = distance / projection; if (min_angle < 0) { min_angle = angle; - } else if(angle < min_angle) { + } else if (angle < min_angle) { min_angle = angle; } } From f3e6aebc58acc7cd3a3d8a63fc5a744349e1c1e2 Mon Sep 17 00:00:00 2001 From: Sanat Dhanyamraju <53443682+sanatd33@users.noreply.github.com> Date: Mon, 29 Jan 2024 17:55:25 -0500 Subject: [PATCH 3/3] fixed redundant if statement --- soccer/src/soccer/strategy/agent/position/offense.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index b0e75a79707..2d97c3d851c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -254,9 +254,7 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry: return -1.0; } double angle = distance / projection; - if (min_angle < 0) { - min_angle = angle; - } else if (angle < min_angle) { + if ((min_angle < 0) || (angle < min_angle)) { min_angle = angle; } }