From c5abdd118e849897869bb6f7e384f82bcf92d40d Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Thu, 11 Jun 2020 03:51:03 -0400
Subject: [PATCH] Fixed unresponsive SIGKILL exit

---
 stretch_core/nodes/stretch_driver | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver
index b400c38..b117cbc 100755
--- a/stretch_core/nodes/stretch_driver
+++ b/stretch_core/nodes/stretch_driver
@@ -17,6 +17,7 @@ from std_msgs.msg import Header
 import tf2_ros
 import tf_conversions
 
+from stretch_body.hello_utils import ThreadServiceExit
 import stretch_body.robot as rb
 import math
 import numpy as np
@@ -1383,17 +1384,17 @@ class StretchBodyNode:
                                                     Trigger,
                                                     self.stop_the_robot_callback)
 
-        
-        # start loop to command the mobile base velocity, publish
-        # odometry, and publish joint states
-        while not rospy.is_shutdown():
-            self.command_mobile_base_velocity_and_publish_state()
-            command_base_velocity_and_publish_joint_state_rate.sleep()
+        try:
+            # start loop to command the mobile base velocity, publish
+            # odometry, and publish joint states
+            while not rospy.is_shutdown():
+                self.command_mobile_base_velocity_and_publish_state()
+                command_base_velocity_and_publish_joint_state_rate.sleep()
+        except (rospy.ROSInterruptException, ThreadServiceExit):
+            self.robot.stop()
+            rospy.signal_shutdown("stretch_driver shutdown")
 
 
 if __name__ == '__main__':
-    try:
-        node = StretchBodyNode()
-        node.main()
-    except rospy.ROSInterruptException:
-        pass
+    node = StretchBodyNode()
+    node.main()