resolved pr comments

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

@@ -13,16 +13,12 @@ using namespace rj_geometry;
 namespace planning {
 
 Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
-    SPDLOG_INFO("Robot {} state {}", plan_request.shell_id, current_state_);
-
     if (plan_request.play_state_ == PlayState::halt() ||
         plan_request.play_state_ == PlayState::stop()) {
         return Trajectory{};
     }
 
     const BallState& ball = plan_request.world_state->ball;
-    // const MotionCommand& command = plan_request.motion_command;
-    // const RobotInstant& start_instant = plan_request.start;
 
     if (!average_ball_vel_initialized_) {
         average_ball_vel_ = ball.velocity;
@@ -37,18 +33,10 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
         average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8);
     }
 
-    // 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));
-
-    // // only plan line kick if not is_done
-    // if (!this->is_done()) {
-    state_transition(ball, plan_request.start);
+    // only process state transition if already started the planning
+    if (has_created_plan) {
+        process_state_transition();
+    }
     switch (current_state_) {
         case INITIAL_APPROACH:
             prev_path_ = initial(plan_request);
@@ -58,21 +46,21 @@ 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
+    // Getting ball info
     const BallState& ball = plan_request.world_state->ball;
 
-    // // Creating a modified plan_request to send to PathTargetPlanner
+    // Creating a modified plan_request to send to PathTargetPlanner
     PlanRequest modified_request = plan_request;
 
     // Where to navigate to
-    auto distance_from_ball = kBallRadius + kRobotRadius + 0.05;
+    auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy;
 
-    auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{0.5}).position;
+    auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position;
 
     auto goal_to_ball = (plan_request.motion_command.target.position - ball_position);
     auto offset_from_ball = distance_from_ball * goal_to_ball.normalized();
@@ -95,12 +83,12 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
 
     const BallState& ball = plan_request.world_state->ball;
 
-    // // Creating a modified plan_request to send to PathTargetPlanner
+    // Creating a modified plan_request to send to PathTargetPlanner
     PlanRequest modified_request = plan_request;
 
     // velocity
     auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
-    auto vel = goal_to_ball.normalized() * 1.0;
+    auto vel = goal_to_ball.normalized() * kFinalRobotSpeed;
 
     LinearMotionInstant target{ball.position, vel};
     MotionCommand modified_command{"path_target", target,
@@ -113,7 +101,7 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
     return path;
 }
 
-void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) {
+void LineKickPathPlanner::process_state_transition() {
     if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) {
         current_state_ = FINAL_APPROACH;
     }
@@ -122,7 +110,7 @@ void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_in
 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

@@ -34,6 +34,7 @@ class LineKickPathPlanner : public PathPlanner {
         target_kick_pos_ = std::nullopt;
         current_state_ = INITIAL_APPROACH;
         average_ball_vel_initialized_ = false;
+        has_created_plan = false;
     }
     [[nodiscard]] bool is_done() const override;
 
@@ -44,9 +45,15 @@ class LineKickPathPlanner : public PathPlanner {
     CollectPathPlanner collect_planner_{};
     Trajectory prev_path_;
 
-    static constexpr double IS_DONE_BALL_VEL = 1.5;
+    // 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,
@@ -55,7 +62,7 @@ class LineKickPathPlanner : public PathPlanner {
     Trajectory final(const PlanRequest& plan_request);
     // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet
     // static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
-    void state_transition(BallState ball, RobotInstant start_instant);
+    void process_state_transition();
 
     // PlayState::State current_state_;
 };