Adding Seeking

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_;
@@ -29,8 +29,8 @@
     double distance_to_ball = robot_position.dist_to(ball_position);
 
     if (current_state_ == IDLING) {
-        send_scorer_request();
-        next_state = SEARCHING;
+        // send_scorer_request();
+        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) {
@@ -77,6 +77,20 @@
         if (check_is_done()) {
             next_state = IDLING;
         }
+    } else if (current_state_ == AWAITING_SEND_PASS) {
+        // Only sends a pass request every 0.2 seconds
+        auto cur = std::chrono::high_resolution_clock::now();
+        auto diff = cur - start;
+        if (distance_to_ball < ball_lost_distance_ && diff.count() > 0.2) {
+            Position::broadcast_direct_pass_request();
+            start = std::chrono::high_resolution_clock::now();
+        }
+    } 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;
@@ -94,7 +108,6 @@
         intent.motion_command = empty_motion_cmd;
         return intent;
     } else if (current_state_ == PASSING) {
-        // attempt to pass the ball to the target robot
         rj_geometry::Point target_robot_pos =
             last_world_state_->get_robot(true, target_robot_id).pose.position();
         planning::LinearMotionInstant target{target_robot_pos};
@@ -174,12 +187,204 @@
             planning::MotionCommand{"path_target", current_location_instant, face_ball};
         intent.motion_command = face_ball_cmd;
         return intent;
+    } else if (current_state_ == AWAITING_SEND_PASS) {
+        auto empty_motion_cmd = planning::MotionCommand{};
+        intent.motion_command = empty_motion_cmd;
+        return intent;
+    } else if (current_state_ == SEEKING) {
+        // Determine target position for seeking
+        rj_geometry::Point current_loc =
+            last_world_state_->get_robot(true, robot_id_).pose.position();
+        rj_geometry::Point curr_target = intent.motion_command.target.position;
+        // Only look for a new position if we are done with current movement
+        if (check_is_done() || target_pt == rj_geometry::Point{0, 0}) {
+            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;
     }
 
     // should be impossible to reach, but this is an EmptyMotionCommand
     return std::nullopt;
 }
 
+rj_geometry::Point Offense::get_open_point(const WorldState* world_state,
+                                           rj_geometry::Point current_loc,
+                                           FieldDimensions field_dimensions) {
+    return Offense::calculate_open_point(3.0, .2, current_loc, world_state, field_dimensions);
+}
+
+rj_geometry::Point Offense::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 = max_los(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)};
+
+        for (auto point : check_points) {
+            curr_val = max_los(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 Offense::random_noise(double prec) {
+    double x = (double)rand() / RAND_MAX * prec;
+    double y = (double)rand() / RAND_MAX * prec;
+    return rj_geometry::Point{x, y};
+}
+
+rj_geometry::Point Offense::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 Offense::max_los(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;
+}
+
 void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) {
     Position::receive_communication_response(response);
 
@@ -205,6 +410,33 @@
                    std::get_if<communication::ResetScorerRequest>(&request.request)) {
         communication::Acknowledge response = receive_reset_scorer_request();
         comm_response.response = response;
+    } else if (const communication::PassRequest* pass_request =
+                   std::get_if<communication::PassRequest>(&request.request)) {
+        // If the robot recieves a PassRequest, only process it if we are oppen
+
+        rj_geometry::Point robot_position =
+            last_world_state_->get_robot(true, robot_id_).pose.position();
+        rj_geometry::Point from_robot_position =
+            last_world_state_->get_robot(true, pass_request->from_robot_id).pose.position();
+        rj_geometry::Segment pass_path{from_robot_position, robot_position};
+        double min_robot_dist = 10000;
+        float min_path_dist = 10000;
+
+        // Calculates the minimum distance from the current robot to all other robots
+        // Also calculates the minimum distance from another robot to the passing line
+        for (auto bot : last_world_state_->their_robots) {
+            rj_geometry::Point opp_pos = bot.pose.position();
+            min_robot_dist = std::min(min_robot_dist, robot_position.dist_to(opp_pos));
+            min_path_dist = std::min(min_path_dist, pass_path.dist_to(opp_pos));
+        }
+
+        // If the current robot is far enough away from other robots and there are no other robots
+        // in the passing line, process the request Currently, max_receive_distance is used to
+        // determine when we are open, but this may need to change
+        if (min_robot_dist > max_receive_distance && min_path_dist > max_receive_distance) {
+            communication::PassResponse response = Position::receive_pass_request(*pass_request);
+            comm_response.response = response;
+        }
     }
 
     return comm_response;

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

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <chrono>
 #include <cmath>
 
 #include <rclcpp/rclcpp.hpp>
@@ -34,21 +35,24 @@
     void derived_acknowledge_ball_in_transit() override;
 
 private:
+    rj_geometry::Point target_pt{0.0, 0.0};
     bool kicking_{true};
 
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
     // TODO (Kevin): strategy design pattern for BallHandler/Receiver
 
     enum State {
-        IDLING,          // simply staying in place
-        SEARCHING,       // moving around on the field to get open
-        PASSING,         // physically kicking the ball towards another robot
-        PREPARING_SHOT,  // pivot around ball in preparation for shot
-        SHOOTING,        // physically kicking the ball towards the net
-        RECEIVING,       // physically intercepting the ball from a pass (gets possession)
-        STEALING,        // attempting to intercept the ball from the other team
-        FACING,          // turning to face the ball
-        SCORER,          // overrides everything and will attempt to steal the bal and shoot it
+        IDLING,              // simply staying in place
+        SEARCHING,           // moving around on the field to get open
+        PASSING,             // physically kicking the ball towards another robot
+        PREPARING_SHOT,      // pivot around ball in preparation for shot
+        SHOOTING,            // physically kicking the ball towards the net
+        RECEIVING,           // physically intercepting the ball from a pass (gets possession)
+        STEALING,            // attempting to intercept the ball from the other team
+        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();
@@ -61,6 +65,23 @@
     bool scorer_ = false;
     bool last_scorer_ = false;
 
+    rj_geometry::Point get_open_point(const WorldState* world_state,
+                                      rj_geometry::Point current_position,
+                                      FieldDimensions field_dimensions);
+
+    rj_geometry::Point correct_point(rj_geometry::Point p, FieldDimensions field_dimensions);
+    rj_geometry::Point random_noise(double prec);
+    rj_geometry::Point calculate_open_point(double, double, rj_geometry::Point,
+                                            const WorldState* world_state,
+                                            FieldDimensions field_dimensions);
+    double max_los(rj_geometry::Point, rj_geometry::Point, const WorldState* world_state);
+
+    int seeker_pos_;
+    std::string offense_type_;
+
+    std::chrono::time_point<std::chrono::high_resolution_clock> start =
+        std::chrono::high_resolution_clock::now();
+
     /**
      * @brief Send request to the other robots to see if this robot should be the scorer
      *

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

@@ -116,12 +116,12 @@ void Position::receive_communication_response(communication::AgentPosResponseWra
 communication::PosAgentResponseWrapper Position::receive_communication_request(
     communication::AgentPosRequestWrapper request) {
     communication::PosAgentResponseWrapper comm_response{};
-    if (const communication::PassRequest* pass_request =
-            std::get_if<communication::PassRequest>(&request.request)) {
-        communication::PassResponse pass_response = receive_pass_request(*pass_request);
-        comm_response.response = pass_response;
-    } else if (const communication::IncomingBallRequest* incoming_ball_request =
-                   std::get_if<communication::IncomingBallRequest>(&request.request)) {
+    // if (const communication::PassRequest* pass_request =
+    //         std::get_if<communication::PassRequest>(&request.request)) {
+    //     communication::PassResponse pass_response = receive_pass_request(*pass_request);
+    //     comm_response.response = pass_response;
+    if (const communication::IncomingBallRequest* incoming_ball_request =
+            std::get_if<communication::IncomingBallRequest>(&request.request)) {
         communication::Acknowledge incoming_pass_acknowledge =
             acknowledge_pass(*incoming_ball_request);
         comm_response.response = incoming_pass_acknowledge;
@@ -155,14 +155,26 @@ void Position::send_direct_pass_request(std::vector<u_int8_t> target_robots) {
     communication_request_ = communication_request;
 }
 
+void Position::broadcast_direct_pass_request() {
+    communication::PassRequest pass_request{};
+    communication::generate_uid(pass_request);
+    pass_request.direct = true;
+    pass_request.from_robot_id = robot_id_;
+
+    communication::PosAgentRequestWrapper communication_request{};
+    communication_request.request = pass_request;
+    communication_request.urgent = true;
+    communication_request.broadcast = true;
+    communication_request_ = communication_request;
+}
+
 communication::PassResponse Position::receive_pass_request(
     communication::PassRequest pass_request) {
     communication::PassResponse pass_response{};
     communication::generate_uid(pass_response);
 
     if (pass_request.direct) {
         // Handle direct pass request
-        // TODO: Make this rely on actually being open
         pass_response.direct_open = true;
     } else {
         // TODO: Handle indirect pass request