diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 4cb3b327cc1..2d97c3d851c 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,52 @@ 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) || (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 *