[ros2topic] fix pub rate reduced issue

Attempt sleep at the specified rate. sleep() takes into account the time elapsed since the last successful sleep().

ref to https://github.com/ros/ros_comm/blob/ros_comm-1.8.15/clients/rospy/src/rospy/timer.py#L62

Signed-off-by: Chris Ye <chris.ye@intel.com>

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
+    else:
+        # 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()