From aed23cc43f0ade046cceb660ee7241647222f47f Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Fri, 27 Mar 2020 12:04:32 +0100 Subject: [PATCH] listen to 'fake_e_stop' and change availability accordingly (cherry picked from commit 54740094729b9340000d2cc42f5a01f170392966) --- .../industrial_robot_simulator | 36 ++++++++++++++++--- 1 file changed, 32 insertions(+), 4 deletions(-) diff --git a/industrial_robot_simulator/industrial_robot_simulator b/industrial_robot_simulator/industrial_robot_simulator index 83df3cdf..26d0606e 100755 --- a/industrial_robot_simulator/industrial_robot_simulator +++ b/industrial_robot_simulator/industrial_robot_simulator @@ -49,6 +49,7 @@ from industrial_msgs.msg import RobotStatus # Subscribe from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint +from std_msgs.msg import Bool # Services from industrial_msgs.srv import GetRobotInfo, GetRobotInfoResponse @@ -270,7 +271,11 @@ class IndustrialRobotSimulatorNode(): # retrieve update rate motion_update_rate = rospy.get_param('motion_update_rate', 100.); #set param to 0 to ignore interpolated motion self.motion_ctrl = MotionControllerSimulator(num_joints, initial_joint_state, update_rate=motion_update_rate) - + + # React to published e-stop + self.e_stop_state = False + self.e_stop_sub = rospy.Subscriber('fake_e_stop', Bool, self.e_stop_changed_callback) + # Published to joint states rospy.logdebug("Creating joint state publisher") self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) @@ -363,9 +368,9 @@ class IndustrialRobotSimulatorNode(): try: msg = RobotStatus() msg.mode.val = RobotMode.AUTO - msg.e_stopped.val = TriState.FALSE + msg.e_stopped.val = TriState.TRUE if self.e_stop_state else TriState.FALSE msg.drives_powered.val = TriState.TRUE - msg.motion_possible.val = TriState.TRUE + msg.motion_possible.val = TriState.FALSE if self.e_stop_state else TriState.TRUE msg.in_motion.val = self.motion_ctrl.is_in_motion() msg.in_error.val = TriState.FALSE msg.error_code = 0 @@ -392,7 +397,8 @@ class IndustrialRobotSimulatorNode(): else: rospy.logdebug('Received empty trajectory while still in motion, stopping current trajectory') self.motion_ctrl.stop() - + elif self.e_stop_state: + rospy.logerr('Received trajectory while e-stopped, ignoring it') else: for point in msg_in.points: point = self._to_controller_order(msg_in.joint_names, point) @@ -479,6 +485,28 @@ class IndustrialRobotSimulatorNode(): # always successfull resp.code.val = ServiceReturnCode.SUCCESS return resp + + """ + e-stop subscription callback (gets called whenever a new e-stop state is received). + + @param msg_in: std_msg.Bool message + @type msg_in: Bool + """ + def e_stop_changed_callback(self, msg_in): + try: + rospy.logdebug('Received new e-stop state "%s", executing callback', 'on' if msg_in.data else 'off') + + if msg_in.data and self.motion_ctrl.is_in_motion(): + rospy.logdebug('Received e-stop while still in motion, stopping current trajectory') + self.motion_ctrl.stop() + + self.e_stop_state = msg_in.data + + except Exception as e: + rospy.logerr('Unexpected exception: %s', e) + + rospy.logdebug('Exiting e-stop callback') + if __name__ == '__main__':