From 349c2bc0aac83528491c0d5d96fa8ede5ec9bd67 Mon Sep 17 00:00:00 2001 From: Sadanand Modak Date: Thu, 26 Jan 2023 17:00:17 -0600 Subject: [PATCH] Bugfix navigation.cc Update: Robot vel constrained by velocity tolerance and not distance tolerance --- src/navigation/navigation.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index 3a000d1..63d34cb 100644 --- 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_) <