Merge Settle into Collect

soccer/src/soccer/planning/planner/collect_path_planner.hpp

@@ -19,11 +19,13 @@ class CollectPathPlanner : public PathPlanner {
     enum CollectPathPathPlannerStates {
         // From start of subbehavior to the start of the slow part of the
         // approach
-        CoarseApproach,
+        COARSE_APPROACH,
+        // Intercepts a moving ball
+        INTERCEPT,
+        // Slows down the velocity of an intercepted ball
+        DAMPEN,
         // From the slow part of the approach to the touching of the ball
-        FineApproach,
-        // From touching the ball to stopped with the ball in the mouth
-        Control
+        FINE_APPROACH,
     };
 
     CollectPathPlanner()
@@ -39,47 +41,66 @@ class CollectPathPlanner : public PathPlanner {
     // and won't intercept ball correctly anymore
     void check_solution_validity(BallState ball, RobotInstant start);
 
-    void process_state_transition(BallState ball, RobotInstant start_instant);
+    void process_state_transition(const PlanRequest& request, BallState ball,
+                                  RobotInstant* start_instant);
 
     Trajectory coarse_approach(
         const PlanRequest& plan_request, RobotInstant start,
         const rj_geometry::ShapeSet& static_obstacles,
         const std::vector<DynamicObstacle>& dynamic_obstacles);
 
+    Trajectory intercept(const PlanRequest& plan_request, RobotInstant start_instant,
+                         const rj_geometry::ShapeSet& static_obstacles,
+                         const std::vector<DynamicObstacle>& dynamic_obstacles);
+
+    // Dampen doesn't need to take obstacles into account.
+    Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant,
+                      const rj_geometry::ShapeSet& static_obstacles,
+                      const std::vector<DynamicObstacle>& dynamic_obstacles);
+
     Trajectory fine_approach(
         const PlanRequest& plan_request, RobotInstant start_instant,
         const rj_geometry::ShapeSet& static_obstacles,
         const std::vector<DynamicObstacle>& dynamic_obstacles);
 
-    Trajectory control(const PlanRequest& plan_request, RobotInstant start,
-                       const Trajectory& partial_path,
-                       const rj_geometry::ShapeSet& static_obstacles,
-                       const std::vector<DynamicObstacle>& dynamic_obstacles);
-
     Trajectory invalid(const PlanRequest& plan_request,
                        const rj_geometry::ShapeSet& static_obstacles,
                        const std::vector<DynamicObstacle>& dynamic_obstacles);
 
     Trajectory previous_;
 
-    CollectPathPathPlannerStates current_state_ = CollectPathPathPlannerStates::CoarseApproach;
+    CollectPathPathPlannerStates current_state_ = CollectPathPathPlannerStates::COARSE_APPROACH;
 
     // Ball Velocity Filtering Variables
     rj_geometry::Point average_ball_vel_;
     bool average_ball_vel_initialized_ = false;
 
     rj_geometry::Point approach_direction_;
-    bool approach_direction_created_ = false;
-
-    bool control_path_created_ = false;
 
     rj_geometry::Point path_coarse_target_;
     bool path_coarse_target_initialized_ = false;
 
-    // is_done vars
-    std::optional<LinearMotionInstant> cached_start_instant_;
-    std::optional<rj_geometry::Point> cached_robot_pos_;
-    std::optional<rj_geometry::Point> cached_ball_pos_;
+    // Intercept Target Filtering Variables
+    rj_geometry::Point avg_instantaneous_intercept_target_;
+    bool first_intercept_target_found_ = false;
+
+    // Only change the target of the path if it changes significantly
+    rj_geometry::Point path_intercept_target_;
+
+    // Have we already made a dampen path
+    bool path_created_for_dampen_ = false;
+
+    // Do we have the ball in the robot
+    bool is_ball_sense_ = false;
+
+    // Threshold for switching from dampen to fine approach
+    static constexpr double kDampenBallSpeedThreshold{0.75};
+
+    // Threshold for ball velocity to try to intercept;
+    static constexpr double kInterceptVelocityThreshold{0.2};
+
+    // Threshold for chasing after the ball instead of intercepting (deg)
+    static constexpr double kChaseAngleThreshold{45};
 };
 
 }  // namespace planning

soccer/src/soccer/planning/planner/settle_path_planner.cpp

@@ -18,6 +18,7 @@ using namespace rj_geometry;
 
 namespace planning {
 
+// Do not use anymore, use collect
 Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) {
     const auto state = plan_request.play_state.state();
     if (state == PlayState::Stop || state == PlayState::Halt) {
@@ -90,7 +91,8 @@ Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) {
                                delta_pos, face_pos);
             break;
         case SettlePathPlannerStates::Dampen:
-            result = dampen(plan_request, start_instant, delta_pos, face_pos);
+            result = dampen(plan_request, start_instant, static_obstacles, dynamic_obstacles,
+                            delta_pos, face_pos);
             break;
         default:
             result = invalid(plan_request, static_obstacles, dynamic_obstacles);
@@ -387,6 +389,8 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
 }
 
 Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInstant start_instant,
+                                     const rj_geometry::ShapeSet& static_obstacles,
+                                     const std::vector<DynamicObstacle>& dynamic_obstacles,
                                      rj_geometry::Point delta_pos, rj_geometry::Point face_pos) {
     // Only run once if we can
 
@@ -467,8 +471,19 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta
     // Target stopping point with 0 speed.
     LinearMotionInstant final_stopping_motion{final_stopping_point};
 
-    Trajectory dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion,
-                                               plan_request.constraints.mot, start_instant.stamp);
+    Trajectory dampen_end;
+
+    if (previous_.empty()) {
+        dampen_end = CreatePath::intermediate(start_instant.linear_motion(), final_stopping_motion,
+                                              plan_request.constraints.mot, start_instant.stamp,
+                                              static_obstacles, dynamic_obstacles,
+                                              plan_request.field_dimensions, plan_request.shell_id);
+    } else {
+        dampen_end = CreatePath::intermediate(
+            previous_.last().linear_motion(), final_stopping_motion, plan_request.constraints.mot,
+            previous_.last().stamp, static_obstacles, dynamic_obstacles,
+            plan_request.field_dimensions, plan_request.shell_id);
+    }
 
     dampen_end.set_debug_text("Damping");
 

soccer/src/soccer/planning/planner/settle_path_planner.hpp

@@ -71,6 +71,8 @@ class SettlePathPlanner : public PathPlanner {
 
     // Dampen doesn't need to take obstacles into account.
     Trajectory dampen(const PlanRequest& plan_request, RobotInstant start_instant,
+                      const rj_geometry::ShapeSet& static_obstacles,
+                      const std::vector<DynamicObstacle>& dynamic_obstacles,
                       rj_geometry::Point delta_pos, rj_geometry::Point face_pos);
 
     Trajectory invalid(const PlanRequest& plan_request,

soccer/src/soccer/planning/primitives/create_path.cpp

@@ -125,8 +125,8 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
             // If the trajectory does not hit an obstacle, it is valid
             if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) {
                 auto angle = (final_inter - start.position).angle();
-                cached_intermediate_tuple_[robot_id] = {
-                    abs(angle), (final_inter - start.position).mag(), signbit(angle) ? -1 : 1};
+                cached_intermediate_tuple_[robot_id] = {abs(angle), signbit(angle) ? -1 : 1,
+                                                        (final_inter - start.position).mag()};
                 return trajectory;
             }
         }

soccer/src/soccer/planning/trajectory.cpp

@@ -23,12 +23,12 @@ Trajectory::Trajectory(Trajectory a, const Trajectory& b) {
 
     using rj_geometry::Point;
     if (!a_end.position().near_point(b_begin.position(), 1e-6) ||
-        !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-6) ||
+        !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-1) ||
         a_end.stamp != b_begin.stamp) {
         SPDLOG_ERROR("points near? {}, vels near? {}, timestamps match? {}",
-                     !a_end.position().near_point(b_begin.position(), 1e-6),
-                     !a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-6),
-                     a_end.stamp != b_begin.stamp);
+                     a_end.position().near_point(b_begin.position(), 1e-6),
+                     a_end.linear_velocity().near_point(b_begin.linear_velocity(), 1e-1),
+                     a_end.stamp == b_begin.stamp);
         throw std::invalid_argument(
             "Cannot splice trajectories a and b, where a.last() != b.first()");
     }

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

@@ -99,8 +99,7 @@ std::optional<RobotIntent> SoloOffense::state_to_task(RobotIntent intent) {
             robotToBall = robotToBall.normalized(length);
             planning::LinearMotionInstant target{
                 last_world_state_->get_robot(true, robot_id_).pose.position() + robotToBall};
-            auto pivot_cmd =
-                planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true};
+            auto pivot_cmd = planning::MotionCommand{"collect"};
             intent.motion_command = pivot_cmd;
             intent.dribbler_speed = 255;
             return intent;
@@ -123,7 +122,7 @@ std::optional<RobotIntent> SoloOffense::state_to_task(RobotIntent intent) {
             planning::LinearMotionInstant target{calculate_best_shot()};
             // planning::LinearMotionInstant target{last_world_state_->ball.position};
             auto kick_cmd =
-                planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true};
+                planning::MotionCommand{"line_kick", target, planning::FaceTarget{}, true};
             intent.motion_command = kick_cmd;
             intent.shoot_mode = RobotIntent::ShootMode::KICK;
             intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;