Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

industrial_robot_simulator: listen to 'fake_e_stop' and change availability accordingly #254

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open

industrial_robot_simulator: listen to 'fake_e_stop' and change availability accordingly #254

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 32 additions & 4 deletions industrial_robot_simulator/industrial_robot_simulator
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it be better to not call this fake_e_stop? The whole script is already a simulator/simulation of a real robot, and those typically don't have fake_e_stop topics.

If we want to add this here, I would propose the rename the subscribed topic. If the producing side would like to call the topic fake_e_stop, remapping could be used.


# Published to joint states
rospy.logdebug("Creating joint state publisher")
self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
Expand Down Expand Up @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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__':
Expand Down