Best shot (#2168)

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: petergarud <[email protected]> Co-authored-by: sid-parikh <[email protected]> Co-authored-by: Sid Parikh <[email protected]>
RoboJackets · Feb 1, 2024 · e737c47 · e737c47
1 parent 3748541
commit e737c47
Show file tree

Hide file tree

Showing 4 changed files with 90 additions and 55 deletions.
diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp
@@ -13,6 +13,9 @@ using namespace rj_geometry;
 namespace planning {
 
 Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
+    // If we are not allowed to touch the ball, this planner always fails
+    // This is preferred to simply ending the planner because it is possible (likely)
+    // that strategy re-requests the same planner anyway.
     if (plan_request.play_state == PlayState::halt() ||
         plan_request.play_state == PlayState::stop()) {
         return Trajectory{};
@@ -29,14 +32,10 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
         //
         // e.g. new_avg_vel = (0.8 * avg_vel) + (0.2 * new_vel)
         //
-        // TODO(Kevin): make this gain a ROS param like collect
         average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8);
     }
 
-    // only process state transition if already started the planning
-    if (has_created_plan) {
-        process_state_transition();
-    }
+    process_state_transition();
     switch (current_state_) {
         case INITIAL_APPROACH:
             prev_path_ = initial(plan_request);
@@ -46,62 +45,57 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
             break;
     }
     prev_path_.stamp(RJ::now());
-    has_created_plan = true;
     return prev_path_;
 }
 
 Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) {
     // Getting ball info
     const BallState& ball = plan_request.world_state->ball;
 
-    // Creating a modified plan_request to send to PathTargetPlanner
-    PlanRequest modified_request = plan_request;
-
-    // Where to navigate to
+    // Distance to stay away from the ball
     auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy;
 
+    // In case the ball is (slowly) moving
     auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position;
 
+    // Along the vector from the goal to ball
     auto goal_to_ball = (plan_request.motion_command.target.position - ball_position);
     auto offset_from_ball = distance_from_ball * goal_to_ball.normalized();
 
-    // Updating the motion command
+    // Create an updated MotionCommand and forward to PathTargetPathPlaner
+    PlanRequest modified_request = plan_request;
+
     LinearMotionInstant target{ball_position - offset_from_ball};
 
     MotionCommand modified_command{"path_target", target,
                                    FacePoint{plan_request.motion_command.target.position}};
     modified_request.motion_command = modified_command;
 
-    // Getting the new path from PathTargetPlanner
-    Trajectory path = path_target_.plan(modified_request);
-
-    return path;
+    return path_target_.plan(modified_request);
 }
 
 Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
-    // remove ball from obstacles not needed?
-
     const BallState& ball = plan_request.world_state->ball;
 
-    // Creating a modified plan_request to send to PathTargetPlanner
-    PlanRequest modified_request = plan_request;
-
-    // velocity
+    // Velocity is the speed (parameter) times the unit vector in the correct direction
     auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
     auto vel = goal_to_ball.normalized() * kFinalRobotSpeed;
 
+    // Create an updated MotionCommand and forward to PathTargetPathPlaner
+    PlanRequest modified_request = plan_request;
+
     LinearMotionInstant target{ball.position, vel};
+
     MotionCommand modified_command{"path_target", target,
-                                   FacePoint{plan_request.motion_command.target.position}, true};
+                                   FacePoint{plan_request.motion_command.target.position}};
     modified_request.motion_command = modified_command;
 
-    // Getting the new path from PathTargetPlanner
-    Trajectory path = path_target_.plan(modified_request);
-
-    return path;
+    return path_target_.plan(modified_request);
 }
 
 void LineKickPathPlanner::process_state_transition() {
+    // Let PathTarget decide when the first stage is done
+    // Possible problem: can PathTarget get stuck and loop infinitely?
     if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) {
         current_state_ = FINAL_APPROACH;
     }

diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp
@@ -7,19 +7,22 @@
 #include "planning/planner/path_target_path_planner.hpp"
 #include "planning/trajectory.hpp"
 
-class Configuration;
-class ConfigDouble;
-
 namespace planning {
 
 /**
  * PathPlanner which plans a path to line kick a ball.
- * Uses the System State object to get the position of the ball
- * and predict its Path. It chooses the closest intersection point
- * with the ball Path it can reach in time and plans a Path so the
- * ball and robot intersect at the same time.
+ * It takes the ball's current trajectory and plans a path for the robot
+ * to intersect with the ball.
+ *
+ * It is in two stages. The first stage drives up to the ball, trying to avoid
+ * accidentally tapping it out of place. The second stage drives directly
+ * into the ball, hopefully kicking it if the hardware has been instructed
+ * to kick.
  *
- * TODO(Kyle): Overhaul this entire planner. It's sketchy right now.
+ * Because of the two-stage system, this planner is *stateful*. It ought
+ * not to be destructed and re-constructed during a single execution;
+ * the best approach is to call plan() on each tick.
+ * However, it also shouldn't *completely* break if it is reset.
  *
  * Params taken from MotionCommand:
  *   target.position - planner will kick to this point
@@ -31,40 +34,49 @@ class LineKickPathPlanner : public PathPlanner {
 
     void reset() override {
         prev_path_ = {};
-        target_kick_pos_ = std::nullopt;
         current_state_ = INITIAL_APPROACH;
         average_ball_vel_initialized_ = false;
-        has_created_plan = false;
     }
+
     [[nodiscard]] bool is_done() const override;
 
 private:
     enum State { INITIAL_APPROACH, FINAL_APPROACH };
-    State current_state_ = INITIAL_APPROACH;
+
+    State current_state_{INITIAL_APPROACH};
+
     PathTargetPathPlanner path_target_{};
     CollectPathPlanner collect_planner_{};
     Trajectory prev_path_;
 
     // These constants could be tuned more
-    static constexpr double kIsDoneBallVel = 1.5;
-    static constexpr double kFinalRobotSpeed = 1.0;
-    static constexpr double kPredictIn = 0.5;  // seconds
-    static constexpr double kAvoidBallBy = 0.05;
+    static constexpr double kIsDoneBallVel{1.5};
+    static constexpr double kFinalRobotSpeed{1.0};
+    static constexpr double kPredictIn{0.5};  // seconds
+    static constexpr double kAvoidBallBy{0.05};
+    static constexpr double kLowPassFilterGain{0.2};
 
     rj_geometry::Point average_ball_vel_;
     bool average_ball_vel_initialized_ = false;
-    bool has_created_plan = false;
-    std::optional<rj_geometry::Point> target_kick_pos_;
 
-    // Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant,
-    // ShapeSet static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
+    /**
+     * Returns the trajectory during the initial stage.
+     * Uses PathTargetPathPlanner to draw a path to the spot to kick from.
+     * Avoids the ball
+     */
     Trajectory initial(const PlanRequest& plan_request);
+
+    /**
+     * Returns the trajectory during the final stage.
+     * Uses PathTargetPathPlanner to draw a path directly into the ball.
+     * Tries to hit the ball with the mouth of the robot.
+     */
     Trajectory final(const PlanRequest& plan_request);
-    // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet
-    // static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
-    void process_state_transition();
 
-    // PlayState::State current_state_;
+    /**
+     * Decides if the intial approach is complete and updates internal state as necessary.
+     */
+    void process_state_transition();
 };
 
 }  // namespace planning
diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp
@@ -139,10 +139,7 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
         intent.dribbler_speed = 255.0;
         return intent;
     } else if (current_state_ == SHOOTING) {
-        rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc();
-        rj_geometry::Point scoring_point =
-            their_goal_pos + field_dimensions_.goal_width() * 3.0 / 8.0;
-        planning::LinearMotionInstant target{scoring_point};
+        planning::LinearMotionInstant target{calculate_best_shot()};
         auto line_kick_cmd = planning::MotionCommand{"line_kick", target};
         intent.motion_command = line_kick_cmd;
         intent.shoot_mode = RobotIntent::ShootMode::KICK;
@@ -416,4 +413,28 @@ void Offense::derived_acknowledge_ball_in_transit() {
     chasing_ball = false;
 }
 
+rj_geometry::Point Offense::calculate_best_shot() {
+    // Goal location
+    rj_geometry::Point their_goal_pos = field_dimensions_.our_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;
+
+    rj_geometry::Point best_shot = their_goal_pos;
+    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;
+            best_shot = curr_point;
+        }
+        curr_point = curr_point + increment;
+    }
+    return best_shot;
+}
+
 }  // namespace strategy
diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp
@@ -71,8 +71,6 @@ class Offense : public Position {
 
     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
      *
@@ -114,6 +112,16 @@ class Offense : public Position {
      * status
      */
     communication::Acknowledge receive_reset_scorer_request();
+
+    /**
+     * @brief Calcualtes the best location for a shot
+     */
+    rj_geometry::Point calculate_best_shot();
+
+    /**
+     * @brief Calculates the distance of vector from other team's closest robot
+     */
+    double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point heaad);
 };
 
 }  // namespace strategy