Merge pull request #44 from ut-amrl/navigation-bugfix-vel-tol

Bugfix navigation.cc
ut-amrl · May 1, 2023 · 1d66b2e · 1d66b2e
2 parents 5374d16 + 349c2bc
commit 1d66b2e
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -882,7 +882,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_) <