Adding Seeking

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
         )

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

@@ -14,7 +14,7 @@
     return state_to_task(intent);
 }
 
 Offense::State Offense::update_state() {
     State next_state = current_state_;
     // handle transitions between current state
     WorldState* world_state = this->last_world_state_;
@@ -30,7 +30,7 @@
 
     if (current_state_ == IDLING) {
         // send_scorer_request();
-        next_state = AWAITING_SEND_PASS;
+        next_state = SEEKING;
     } else if (current_state_ == SEARCHING) {
         if (scorer_) {
             next_state = STEALING;
@@ -58,7 +58,7 @@
         }
     } 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 @@
         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 next_state;
@@ -181,6 +187,13 @@
         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) {
+            Seeking 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>
@@ -13,6 +14,7 @@
 #include "rj_common/time.hpp"
 #include "rj_geometry/geometry_conversions.hpp"
 #include "rj_geometry/point.hpp"
+#include "seeking.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
      *

soccer/src/soccer/strategy/agent/position/seeking.cpp

@@ -0,0 +1,190 @@
+#include "seeking.hpp"
+
+namespace strategy {
+
+Seeking::Seeking(int robot_id) { robot_id_ = robot_id; }
+
+std::optional<RobotIntent> 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<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 = 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

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

@@ -0,0 +1,101 @@
+#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 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<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,
+                                      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