Rewrite linekicker

rj_constants/include/rj_constants/constants.hpp

@@ -30,5 +30,8 @@ constexpr float kDotsSmallOffset = 0.035;
 constexpr float kDotsLargeOffset = 0.054772;
 constexpr float kDotsRadius = 0.02;
 
+/** constants for planning */
+constexpr double kAvoidBallDistance = 0.01;
+
 const std::string kTeamNameLower = "robojackets";
 const std::string kTeamName = "RoboJackets";

soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

@@ -1,4 +1,4 @@
-#include "line_kick_path_planner.hpp"
+#include "planning/planner/line_kick_path_planner.hpp"
 
 #include <rj_geometry/util.hpp>
 
@@ -13,30 +13,13 @@ using namespace rj_geometry;
 namespace planning {
 
 Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
-    // TODO(?): ros param these
-    const float approach_speed = 0.05;
-
-    const float ball_avoid_distance = 0.10;
-
-    const MotionCommand& command = plan_request.motion_command;
-
-    if (plan_request.virtual_obstacles.hit(plan_request.start.position())) {
-        prev_path_ = Trajectory{};
-        return prev_path_;
+    if (plan_request.play_state == PlayState::halt() ||
+        plan_request.play_state == PlayState::stop()) {
+        return Trajectory{};
     }
 
-    if (target_kick_pos_.has_value() &&
-        command.target.position.dist_to(target_kick_pos_.value()) > 0.1) {
-        prev_path_ = Trajectory{};
-        target_kick_pos_ = std::nullopt;
-    }
+    const BallState& ball = plan_request.world_state->ball;
 
-    const RobotInstant& start_instant = plan_request.start;
-    const auto& motion_constraints = plan_request.constraints.mot;
-    const auto& rotation_constraints = plan_request.constraints.rot;
-    const auto& ball = plan_request.world_state->ball;
-
-    // track ball velocity to know if done or not
     if (!average_ball_vel_initialized_) {
         average_ball_vel_ = ball.velocity;
         average_ball_vel_initialized_ = true;
@@ -50,230 +33,84 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
         average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8);
     }
 
-    if (prev_path_.empty()) {
-        final_approach_ = false;
+    // only process state transition if already started the planning
+    if (has_created_plan) {
+        process_state_transition();
     }
-
-    ShapeSet static_obstacles;
-    std::vector<DynamicObstacle> dynamic_obstacles;
-    fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, nullptr);
-
-    auto obstacles_with_ball = static_obstacles;
-    const RJ::Time cur_time = start_instant.stamp;
-    obstacles_with_ball.add(
-        make_shared<Circle>(ball.predict_at(cur_time).position, ball_avoid_distance));
-
-    // This segfaults. Please rewrite the entire thing.
-#if 0
-    if (final_approach_ && target_kick_pos_) {
-        RJ::Seconds duration_into_path = cur_time - prev_path_.begin_time();
-
-        RobotInstant target = prev_path_.last();
-        RJ::Time time = ball.query_time_near(*target_kick_pos_);
-
-        auto time_left = prev_path_.duration() - duration_into_path;
-
-        if (time_left < RJ::Seconds(-0.3) || time_left > RJ::Seconds(5.0)) {
-            final_approach_ = false;
-            prev_path_ = {};
-        } else if (time_left < RJ::Seconds(0)) {
-            prev_path_.set_debug_text("reuse past done " + std::to_string(time_left.count()));
-            return prev_path_;
-        } else {
-            RJ::Seconds time_for_ball = time - cur_time;
-            prev_path_.scale_duration(prev_path_.duration() * (time_left / time_for_ball),
-                                      start_instant.stamp);
-            prev_path_.set_debug_text("reuse final slow " + std::to_string(time_for_ball.count()) +
-                                      " " + std::to_string(time_left.count()));
-            prev_path_.stamp(RJ::now());
-            return prev_path_;
-        }
+    switch (current_state_) {
+        case INITIAL_APPROACH:
+            prev_path_ = initial(plan_request);
+            break;
+        case FINAL_APPROACH:
+            prev_path_ = final(plan_request);
+            break;
     }
-#endif
-
-    // only plan line kick if not is_done
-    if (!this->is_done()) {
-        LinearMotionInstant target{
-            ball.position, (command.target.position - ball.position).normalized(approach_speed)};
-
-        auto ball_trajectory = ball.make_trajectory();
-
-        Trajectory path;
-        if (std::abs(target.velocity.angle_between((target.position - start_instant.position()))) >
-            degrees_to_radians(10)) {
-            target.position -= target.velocity.normalized(ball_avoid_distance * 2 + kRobotRadius);
-            if (!prev_path_.empty() &&
-                target.position.dist_to(prev_path_.last().position()) <
-                    Replanner::goal_pos_change_threshold() &&
-                reuse_path_count_ < 20) {
-                target.velocity = prev_path_.last().linear_velocity();
-                reuse_path_count_++;
-            } else {
-                reuse_path_count_ = 0;
-            }
+    prev_path_.stamp(RJ::now());
+    has_created_plan = true;
+    return prev_path_;
+}
 
-            Replanner::PlanParams params{
-                start_instant,
-                target,
-                obstacles_with_ball,
-                dynamic_obstacles,
-                plan_request.constraints,
-                AngleFns::face_angle(ball.position.angle_to(command.target.position))};
-            path = Replanner::create_plan(params, prev_path_);
-            path.set_debug_text("slow ball 1");
-        } else {
-            if (!prev_path_.empty() &&
-                target.position.dist_to(prev_path_.last().position()) <
-                    Replanner::goal_pos_change_threshold() &&
-                reuse_path_count_ < 20) {
-                target.velocity = prev_path_.last().linear_velocity();
-                reuse_path_count_++;
-            } else {
-                reuse_path_count_ = 0;
-            }
+Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) {
+    // Getting ball info
+    const BallState& ball = plan_request.world_state->ball;
 
-            target.position += target.velocity.normalized(kRobotRadius);
+    // Creating a modified plan_request to send to PathTargetPlanner
+    PlanRequest modified_request = plan_request;
 
-            Replanner::PlanParams params{start_instant,
-                                         target,
-                                         static_obstacles,
-                                         dynamic_obstacles,
-                                         plan_request.constraints,
-                                         AngleFns::face_point(command.target.position)};
-            path = Replanner::create_plan(params, prev_path_);
-            path.set_debug_text("slow ball 2");
-        }
-        target_kick_pos_ = command.target.position;
-        path.stamp(RJ::now());
-        prev_path_ = path;
-        return path;
-    }
+    // Where to navigate to
+    auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy;
 
-    return Trajectory{};
-    // (Kevin) pretty sure everything under this line is not used
+    auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position;
 
-    if (!prev_path_.empty() && target_kick_pos_) {
-        auto previous_duration_remaining = prev_path_.end_time() - start_instant.stamp;
+    auto goal_to_ball = (plan_request.motion_command.target.position - ball_position);
+    auto offset_from_ball = distance_from_ball * goal_to_ball.normalized();
 
-        LinearMotionInstant target;
-        RJ::Time intercept_time = ball.query_time_near(*target_kick_pos_, &target.position);
-        RJ::Time end_time_adjusted = prev_path_.end_time() - RJ::Seconds(1.0);
-        if (previous_duration_remaining < RJ::Seconds(0.0)) {
-            target.velocity =
-                (command.target.position - target.position).normalized(approach_speed);
-            target.position -= target.velocity.normalized(kRobotRadius + kBallRadius * 2);
+    // Updating the motion command
+    LinearMotionInstant target{ball_position - offset_from_ball};
 
-            Replanner::PlanParams params{start_instant,
-                                         target,
-                                         static_obstacles,
-                                         dynamic_obstacles,
-                                         plan_request.constraints,
-                                         AngleFns::face_point(command.target.position)};
-            Trajectory path = Replanner::create_plan(params, prev_path_);
+    MotionCommand modified_command{"path_target", target,
+                                   FacePoint{plan_request.motion_command.target.position}};
+    modified_request.motion_command = modified_command;
 
-            if (!path.empty()) {
-                path.set_debug_text(
-                    "FinalPath" + std::to_string(path.duration().count()) + " " +
-                    std::to_string(RJ::Seconds(intercept_time - start_instant.stamp).count()) +
-                    " " + std::to_string(intercept_time.time_since_epoch().count()));
-                path.stamp(RJ::now());
-                prev_path_ = path;
-                return path;
-            }
-        }
+    // Getting the new path from PathTargetPlanner
+    Trajectory path = path_target_.plan(modified_request);
 
-        if (prev_path_.check_time(start_instant.stamp) &&
-            !trajectory_hits_static(prev_path_, static_obstacles, start_instant.stamp, nullptr) &&
-            end_time_adjusted < intercept_time && reuse_path_count_ < 10) {
-            reuse_path_count_++;
-            Point near_point;
-            prev_path_.set_debug_text("Reuse prev_path");
-            if (ball.query_time_near(prev_path_.last().position(), &near_point) >=
-                end_time_adjusted) {
-                return prev_path_;
-            }
-        }
-    }
+    return path;
+}
 
-    Trajectory partial_path;
-    RJ::Seconds partial_path_time = 0ms;
-    auto tmp_start_instant = start_instant;
-    const auto partial_replan_lead_time = RJ::Seconds(Replanner::partial_replan_lead_time());
+Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
+    // remove ball from obstacles not needed?
 
-    if (!prev_path_.empty() && prev_path_.check_time(start_instant.stamp)) {
-        if (start_instant.stamp < prev_path_.end_time() - partial_replan_lead_time * 2) {
-            partial_path = prev_path_.sub_trajectory(
-                start_instant.stamp, start_instant.stamp + partial_replan_lead_time);
-            partial_path_time = partial_replan_lead_time;
-            tmp_start_instant = partial_path.last();
-        }
-    }
+    const BallState& ball = plan_request.world_state->ball;
 
-    for (auto t = RJ::Seconds(0.4); t < RJ::Seconds(6); t += RJ::Seconds(0.2)) {
-        RJ::Time rollout_time = cur_time + t;
+    // Creating a modified plan_request to send to PathTargetPlanner
+    PlanRequest modified_request = plan_request;
 
-        auto ball_state_predicted = ball.predict_at(rollout_time);
-        LinearMotionInstant target{ball_state_predicted.position};
-        target_kick_pos_ = target.position;
-        target.velocity = (command.target.position - target.position).normalized(approach_speed);
-        target.position -= target.velocity.normalized(kRobotRadius + kBallRadius * 2);
+    // velocity
+    auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
+    auto vel = goal_to_ball.normalized() * kFinalRobotSpeed;
 
-        vector<Point> intermediate_points;
-        if (std::abs(target.velocity.angle_between(
-                (target.position - tmp_start_instant.position()))) > degrees_to_radians(60)) {
-            intermediate_points.push_back(
-                target.position -
-                target.velocity.normalized(kRobotRadius * 2.0 + kBallRadius * 2.0));
-        }
+    LinearMotionInstant target{ball.position, vel};
+    MotionCommand modified_command{"path_target", target,
+                                   FacePoint{plan_request.motion_command.target.position}, true};
+    modified_request.motion_command = modified_command;
 
-        Trajectory path = CreatePath::simple(tmp_start_instant.linear_motion(), target,
-                                             plan_request.constraints.mot, tmp_start_instant.stamp,
-                                             intermediate_points);
+    // Getting the new path from PathTargetPlanner
+    Trajectory path = path_target_.plan(modified_request);
 
-        if (!path.empty()) {
-            if (path.duration() + partial_path_time <= t) {
-                if (!partial_path.empty()) {
-                    path = Trajectory(std::move(partial_path), path);
-                }
-                plan_angles(&path, tmp_start_instant, AngleFns::face_point(target.position),
-                            plan_request.constraints.rot);
+    return path;
+}
 
-                path.set_debug_text("FoundPath" + std::to_string(path.duration().count()));
-                reuse_path_count_ = 0;
-                path.stamp(RJ::now());
-                prev_path_ = path;
-                return path;
-            }
-        }
+void LineKickPathPlanner::process_state_transition() {
+    if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) {
+        current_state_ = FINAL_APPROACH;
     }
-
-    auto ball_predicted = ball.predict_at(cur_time);
-    LinearMotionInstant target{ball_predicted.position};
-    target.velocity = (command.target.position - target.position).normalized(approach_speed);
-    target.position -= target.velocity.normalized(kRobotRadius * 3);
-
-    auto ball_path = ball.make_trajectory();
-    dynamic_obstacles.emplace_back(kBallRadius, &ball_path);
-
-    Replanner::PlanParams params{start_instant,
-                                 target,
-                                 static_obstacles,
-                                 dynamic_obstacles,
-                                 plan_request.constraints,
-                                 AngleFns::face_point(command.target.position)};
-    Trajectory path = Replanner::create_plan(params, prev_path_);
-
-    path.set_debug_text("Approaching cautious");
-
-    path.stamp(RJ::now());
-    prev_path_ = path;
-    return path;
 }
 
 bool LineKickPathPlanner::is_done() const {
     // if ball is fast, assume we have kicked it correctly
     // (either way we can't go recapture it)
-    return average_ball_vel_.mag() > IS_DONE_BALL_VEL;
+    return average_ball_vel_.mag() > kIsDoneBallVel;
 }
 
 }  // namespace planning

soccer/src/soccer/planning/planner/line_kick_path_planner.hpp

@@ -2,7 +2,9 @@
 
 #include <optional>
 
+#include "planning/planner/collect_path_planner.hpp"
 #include "planning/planner/path_planner.hpp"
+#include "planning/planner/path_target_path_planner.hpp"
 #include "planning/trajectory.hpp"
 
 class Configuration;
@@ -29,25 +31,40 @@
 
     void reset() override {
         prev_path_ = {};
-        final_approach_ = false;
         target_kick_pos_ = std::nullopt;
-        reuse_path_count_ = 0;
+        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;
+    PathTargetPathPlanner path_target_{};
+    CollectPathPlanner collect_planner_{};
     Trajectory prev_path_;
-    bool final_approach_ = false;
-    std::optional<rj_geometry::Point> target_kick_pos_;
-    int reuse_path_count_ = 0;
 
-    // ball velocity filtering vars
+    // 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;
+
     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);
+    Trajectory initial(const PlanRequest& plan_request);
+    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();
 
-    // TODO(Kevin): make this a common param ("ball is slow" used
-    // in a lot of places)
-    double IS_DONE_BALL_VEL = 0.5;  // m/s
+    // PlayState::State current_state_;
 };
 
 }  // namespace planning