Bugfix navigation.cc

Update: Robot vel constrained by velocity tolerance and not distance tolerance
ut-amrl · Jan 26, 2023 · 349c2bc · 349c2bc
1 parent 7639201
commit 349c2bc
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -878,7 +878,7 @@ bool Navigation::Run(const double& time,
     prev_state = nav_state_;
     if (nav_state_ == NavigationState::kGoto &&
         local_target_.squaredNorm() < Sq(params_.target_dist_tolerance) &&
-        robot_vel_.squaredNorm() < Sq(params_.target_dist_tolerance)) {
+        robot_vel_.squaredNorm() < Sq(params_.target_vel_tolerance)) {
       nav_state_ = NavigationState::kTurnInPlace;
     } else if (nav_state_ == NavigationState::kTurnInPlace &&
           AngleDist(robot_angle_, nav_goal_angle_) <