From a00176fdd1cbe6450a07b062ff30427d5735177d Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Tue, 8 Apr 2025 21:41:00 -0400 Subject: [PATCH 1/3] Adding dynamic thresholding --- soccer/src/soccer/planning/planner/rotate_path_planner.cpp | 5 +++++ soccer/src/soccer/planning/planner/rotate_path_planner.hpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 412cd784b4..d0de2ec2a5 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(); + kIsDoneAngleChangeThresh = max(pow(2, -target_distance), 0.01); + } update_state(); switch (current_state_) { case PIVOT: diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp index 78777225c2..ed26c89400 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 kIsDoneAngleChangeThresh{1.0}; }; } // namespace planning \ No newline at end of file From 19f00c6710a03776edd64dd92a629a85a53fa778 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 8 Apr 2025 21:44:54 -0400 Subject: [PATCH 2/3] Fix Code Style On dynamic-rotate-threshold (#2377) automated style fixes Co-authored-by: rishiso --- soccer/src/soccer/planning/planner/rotate_path_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index d0de2ec2a5..1cac21004a 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -20,7 +20,7 @@ using namespace rj_geometry; Trajectory RotatePathPlanner::plan(const PlanRequest& request) { if (!cached_angle_change_ && request.trigger_mode == RobotIntent::TriggerMode::AT_END) { - double target_distance = + double target_distance = (request.motion_command.target.position - request.start.pose.position()).mag(); kIsDoneAngleChangeThresh = max(pow(2, -target_distance), 0.01); } From a50a3175df72ad82a621c68d408e2566ea38c837 Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Tue, 8 Apr 2025 21:51:24 -0400 Subject: [PATCH 3/3] Name fix --- soccer/src/soccer/planning/planner/rotate_path_planner.cpp | 6 +++--- soccer/src/soccer/planning/planner/rotate_path_planner.hpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp index 1cac21004a..8dcf3ee40b 100644 --- a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -22,7 +22,7 @@ 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(); - kIsDoneAngleChangeThresh = max(pow(2, -target_distance), 0.01); + isDoneAngleChangeThresh = max(pow(2, -target_distance), 0.01); } update_state(); switch (current_state_) { @@ -40,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; } @@ -75,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 ed26c89400..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; - double kIsDoneAngleChangeThresh{1.0}; + double isDoneAngleChangeThresh{1.0}; }; } // namespace planning \ No newline at end of file