From 7366b6f2a43c9389f68bd8954120064341088d5e Mon Sep 17 00:00:00 2001
From: Elvin Yang <eyang@cs.utexas.edu>
Date: Sun, 2 Apr 2023 13:26:30 -0500
Subject: [PATCH] Add stopping condition for terrain-based nav

---
 src/navigation/navigation.cc | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index 9cdc5e7..e7d4a74 100644
--- 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) {