From e45904c0fc6093b0459054f6fe51d15c932c9d2a Mon Sep 17 00:00:00 2001
From: hello-jackson <jhamilton@hello-robot.com>
Date: Tue, 27 Jun 2023 14:18:49 -0700
Subject: [PATCH] extended bug fix to MobileBaseCommandGroup

---
 stretch_core/nodes/command_groups.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py
index 7e10838..45b9929 100644
--- a/stretch_core/nodes/command_groups.py
+++ b/stretch_core/nodes/command_groups.py
@@ -613,6 +613,8 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                                         v_m=self.goal['velocity'],
                                         a_m=self.goal['acceleration'],
                                         contact_thresh=self.goal['contact_threshold'])
+            if self.goal_reached():
+                return False
         return True
 
     def update_execution(self, robot_status, **kwargs):