From f2d63d56a9c79b03f599a32d80e92f786600bcf1 Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Wed, 29 Jul 2020 16:55:19 -0400
Subject: [PATCH] Added service to runstop Stretch

---
 stretch_core/nodes/stretch_driver | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver
index cf5cb62..15c5db4 100755
--- a/stretch_core/nodes/stretch_driver
+++ b/stretch_core/nodes/stretch_driver
@@ -21,6 +21,7 @@ from control_msgs.msg import FollowJointTrajectoryAction
 from control_msgs.msg import FollowJointTrajectoryResult
 
 from std_srvs.srv import Trigger, TriggerResponse
+from std_srvs.srv import SetBool, SetBoolResponse
 
 from nav_msgs.msg import Odometry
 from sensor_msgs.msg import JointState, Imu, MagneticField
@@ -453,6 +454,17 @@ class StretchBodyNode:
             message='Now in position mode.'
             )
 
+    def runstop_service_callback(self, request):
+        if request.data:
+            self.robot.pimu.runstop_event_trigger()
+        else:
+            self.robot.pimu.runstop_event_reset()
+
+        return SetBoolResponse(
+            success=True,
+            message='is_runstopped: {0}'.format(request.data)
+            )
+
     ########### MAIN ############
 
     def main(self):
@@ -579,6 +591,10 @@ class StretchBodyNode:
                                                     Trigger,
                                                     self.stop_the_robot_callback)
 
+        self.runstop_service = rospy.Service('/runstop',
+                                              SetBool,
+                                              self.runstop_service_callback)
+
         try:
             # start loop to command the mobile base velocity, publish
             # odometry, and publish joint states