[joint_state_publisher] Handle time moving backwards

Without this patch, joint_state_publisher dies whenever the ROS time moves backwards (e.g., when running `rosbag play --clock --loop`).
This commit is contained in:
Martin Günther 2017-03-23 14:12:28 +01:00 committed by Chris Lalancette
parent 0a6b4915ce
commit 306e52ef8d
1 changed files with 4 additions and 1 deletions

View File

@ -238,7 +238,10 @@ class JointStatePublisher():
msg.effort[i] = joint['effort']
self.pub.publish(msg)
r.sleep()
try:
r.sleep()
except rospy.exceptions.ROSTimeMovedBackwardsException:
pass
def update(self, delta):
for name, joint in self.free_joints.iteritems():