Adding Seeking (#2160)

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: sanatd33 <[email protected]>
Co-authored-by: jvogt23 <[email protected]>
Co-authored-by: Peter Garud <[email protected]>
Co-authored-by: Sid Parikh <[email protected]>
Co-authored-by: sid-parikh <[email protected]>

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
         )

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;
@@ -196,6 +202,13 @@ std::optional<RobotIntent> 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

soccer/src/soccer/strategy/agent/position/offense.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <chrono>
 #include <cmath>
 
 #include <rclcpp/rclcpp.hpp>
@@ -14,6 +15,7 @@
 #include "rj_constants/constants.hpp"
 #include "rj_geometry/geometry_conversions.hpp"
 #include "rj_geometry/point.hpp"
+#include "seeker.hpp"
 
 namespace strategy {
 
@@ -54,6 +56,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();
@@ -66,8 +69,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
      *

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<RobotIntent> 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<rj_geometry::Point> 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

soccer/src/soccer/strategy/agent/position/seeker.hpp

@@ -0,0 +1,106 @@
+#pragma once
+
+#include <cmath>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <spdlog/spdlog.h>
+
+#include <rj_msgs/action/robot_move.hpp>
+
+#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<RobotIntent> 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