From aa3aad32fe4be2ac8117ce4e7f2c423f29e03735 Mon Sep 17 00:00:00 2001 From: Aoi Nakane Date: Fri, 15 Dec 2023 00:31:01 +0900 Subject: [PATCH] [jsk_topic_tools] Add reset arg to rospy.Timer for rosbag play --loop --- jsk_topic_tools/scripts/boolean_node.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/jsk_topic_tools/scripts/boolean_node.py b/jsk_topic_tools/scripts/boolean_node.py index d5c835b5e..3233e225a 100755 --- a/jsk_topic_tools/scripts/boolean_node.py +++ b/jsk_topic_tools/scripts/boolean_node.py @@ -7,6 +7,8 @@ import sys import rospy +from distutils.version import LooseVersion +import pkg_resources import std_msgs.msg from jsk_topic_tools.eval_utils import expr_eval @@ -65,7 +67,17 @@ def __init__(self): if rate == 0: rospy.logwarn('You cannot set 0 as the rate; change it to 100.') rate = 100 - rospy.Timer(rospy.Duration(1.0 / rate), self.timer_cb) + kwargs = dict( + period=rospy.Duration(1.0 / rate), + callback=self.timer_cb, + oneshot=False, + ) + if (LooseVersion(pkg_resources.get_distribution('rospy').version) >= + LooseVersion('1.12.0')) and rospy.get_param('/use_sim_time', None): + # on >=kinetic, it raises ROSTimeMovedBackwardsException + # when we use rosbag play --loop. + kwargs['reset'] = True + rospy.Timer(**kwargs) def callback(self, topic_name, msg): if isinstance(msg, rospy.msg.AnyMsg):