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