diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 412cd784b4..8dcf3ee40b 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -19,6 +19,11 @@ namespace planning { using namespace rj_geometry; Trajectory RotatePathPlanner::plan(const PlanRequest& request) { + if (!cached_angle_change_ && request.trigger_mode == RobotIntent::TriggerMode::AT_END) { + double target_distance = + (request.motion_command.target.position - request.start.pose.position()).mag(); + isDoneAngleChangeThresh = max(pow(2, -target_distance), 0.01); + } update_state(); switch (current_state_) { case PIVOT: @@ -35,7 +40,7 @@ void RotatePathPlanner::update_state() { return; } current_state_ = abs(cached_angle_change_.value()) < - degrees_to_radians(static_cast(kIsDoneAngleChangeThresh)) + degrees_to_radians(static_cast(isDoneAngleChangeThresh)) ? END : PIVOT; } @@ -70,7 +75,7 @@ Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { Trajectory path{}; if (cached_target_angle_.has_value() && - (*cached_target_angle_ - target_angle) < degrees_to_radians(kIsDoneAngleChangeThresh)) { + (*cached_target_angle_ - target_angle) < degrees_to_radians(isDoneAngleChangeThresh)) { if (cached_path_) { path = cached_path_.value(); } else { diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp index 78777225c2..c82eb418eb 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp @@ -47,6 +47,6 @@ class RotatePathPlanner : public PathPlanner { enum State { PIVOT, END }; RotatePathPlanner::State current_state_ = RotatePathPlanner::State::PIVOT; - static constexpr double kIsDoneAngleChangeThresh{1.0}; + double isDoneAngleChangeThresh{1.0}; }; } // namespace planning \ No newline at end of file