diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 9f93219dc06..f0fbf689c1b 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/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 689e50eda7d..85877fceda5 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -29,8 +29,8 @@ Offense::State Offense::update_state() { double distance_to_ball = robot_position.dist_to(ball_position); if (current_state_ == IDLING) { - send_scorer_request(); - next_state = SHOOTING; + // send_scorer_request(); + next_state = SEEKING; } else if (current_state_ == SEARCHING) { if (scorer_) { next_state = STEALING; @@ -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 < ball_receive_distance_) { + if (distance_to_ball < 2 * ball_receive_distance_) { next_state = IDLING; } } else if (current_state_ == STEALING) { @@ -81,6 +81,12 @@ Offense::State Offense::update_state() { if (distance_to_ball < ball_lost_distance_) { Position::broadcast_direct_pass_request(); } + } 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 SHOOTING; @@ -191,6 +197,13 @@ std::optional Offense::state_to_task(RobotIntent intent) { auto empty_motion_cmd = planning::MotionCommand{}; 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) { + Seeker seeker{robot_id_}; + return seeker.get_task(intent, last_world_state_, this->field_dimensions_); + } } // should be impossible to reach, but this is an EmptyMotionCommand diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 2c13dacef8f..75dc60aabc1 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -13,6 +14,7 @@ #include "rj_common/time.hpp" #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" +#include "seeker.hpp" namespace strategy { @@ -50,6 +52,7 @@ class Offense : public Position { 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(); @@ -62,8 +65,6 @@ class Offense : public Position { bool scorer_ = false; bool last_scorer_ = false; - communication::PassResponse receive_pass_request(communication::PassRequest pass_request); - /** * @brief Send request to the other robots to see if this robot should be the scorer * diff --git a/soccer/src/soccer/strategy/agent/position/seeker.cpp b/soccer/src/soccer/strategy/agent/position/seeker.cpp new file mode 100644 index 00000000000..65467d376a9 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/seeker.cpp @@ -0,0 +1,191 @@ +#include "seeker.hpp" + +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) { + // 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 Seeker::get_open_point(const WorldState* world_state, + rj_geometry::Point current_position, + const FieldDimensions& field_dimensions) const { + 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, + 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; + 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{ + 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 = Seeker::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 Seeker::correct_point(rj_geometry::Point p, + const FieldDimensions& field_dimensions) const { + 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 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 + + // 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 = 0; + 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.1f / 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 diff --git a/soccer/src/soccer/strategy/agent/position/seeker.hpp b/soccer/src/soccer/strategy/agent/position/seeker.hpp new file mode 100644 index 00000000000..e6296b296ae --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/seeker.hpp @@ -0,0 +1,106 @@ +#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 Seeker role provides the implementation for a offensive robot that + * is trying to get open, so that they can receive a pass + */ +class Seeker : public RoleInterface { +public: + 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 + * + * @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, + const FieldDimensions& field_dimensions) const; + + /** + * @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, + const FieldDimensions& field_dimensions) const; + + /** + * @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 + */ + [[nodiscard]] rj_geometry::Point correct_point(rj_geometry::Point point, + const FieldDimensions& field_dimensions) const; + + /** + * @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 + */ + static double eval_point(rj_geometry::Point ball_pos, rj_geometry::Point current_point, + const WorldState* world_state); +}; + +} // namespace strategy