diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index e07bdebea..3cfc70102 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -69,6 +69,35 @@ def main(args): args.node_name, 1. / args.rate, args.print, args.once) +_last_time = 0. + + +def _sleep(period): + """ + Attempt sleep at the specified rate. + + sleep() takes into account the time elapsed since + the last successful sleep() + """ + global _last_time + curr_time = time.time() + + # detect time jumping backwards, handle initial value + if _last_time > curr_time or _last_time == 0: + _last_time = curr_time + + elapsed = curr_time - _last_time + # detect time jumping forwards, as well as loops that are + # inherently too slow + if elapsed > period: + _last_time = curr_time + return + + # sleep remaining time + time.sleep(period - elapsed) + _last_time = _last_time + period + + def publisher( message_type, topic_name, values, node_name, period, print_nth, once ): @@ -109,6 +138,6 @@ def publisher( if once: time.sleep(0.1) # make sure the message reaches the wire before exiting break - time.sleep(period) + _sleep(period) node.destroy_node() rclpy.shutdown()