Add stopping condition for terrain-based nav

ut-amrl · Apr 2, 2023 · 7366b6f · 7366b6f
1 parent 0636856
commit 7366b6f
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -949,8 +949,16 @@ bool Navigation::Run(const double& time,
         if (kDebug) printf("TurnInPlace\n");
         TurnInPlace(cmd_vel, cmd_angle_vel);
       } else {
-        if (kDebug) printf("ObstAv\n");
-        RunObstacleAvoidance(cmd_vel, cmd_angle_vel);
+        if (local_target.norm() < params_.target_dist_tolerance) {
+          // Patch added for terrain-based navigation to stop the robot when it
+          // reaches the goal because the constant curvature arcs are not
+          // truncated to prevent progress away from the goal.
+          if (kDebug) printf("Halt (Terrain, Reached Goal)\n");
+          Halt(cmd_vel, cmd_angle_vel);
+        } else {
+          if (kDebug) printf("ObstAv\n");
+          RunObstacleAvoidance(cmd_vel, cmd_angle_vel);
+        }
       }
     }
   } else if (nav_state_ == NavigationState::kTurnInPlace) {