From 5c4808c700897495e0f81eb236f92cea025aa8df Mon Sep 17 00:00:00 2001
From: Chris Ye <chris.ye@intel.com>
Date: Thu, 30 Aug 2018 13:28:14 +0800
Subject: [PATCH] [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 | 31 ++++++++++++++++++++++++++++++-
 1 file changed, 30 insertions(+), 1 deletion(-)

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()