Adding dynamic thresholding

Author: rishiso

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:

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