diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp index ac4a4189119..1029e52c72d 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/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 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(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 diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp index 75ba5ffdbc7..0278687330b 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/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 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 dynamic_obstacles); - void state_transition(BallState ball, RobotInstant start_instant); + void process_state_transition(); // PlayState::State current_state_; };