From 3d3401e0a63ad323a6570844876842b1dc934639 Mon Sep 17 00:00:00 2001
From: Romain Reignier <rre@robopec.com>
Date: Fri, 26 May 2023 20:21:48 +0200
Subject: [PATCH] Fix log throttling

We were using the old time stamp to compute the next log time
But in case of a pause of the logs (that I have tried to simulate in
the test), after the pause, we need to compute the next log time
from the current time.
---
 rosrust/src/api/ros.rs             |  2 +-
 rosrust/tests/can_throttle_logs.rs | 27 ++++++++++++++++++---------
 2 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/rosrust/src/api/ros.rs b/rosrust/src/api/ros.rs
index a229efd..1342950 100644
--- a/rosrust/src/api/ros.rs
+++ b/rosrust/src/api/ros.rs
@@ -444,7 +444,7 @@ impl Ros {
         match period_logs.get_mut(&key) {
             Some(next_log_time) => {
                 if now >= *next_log_time {
-                    *next_log_time = get_next_log_time(*next_log_time, period);
+                    *next_log_time = get_next_log_time(now, period);
                     self.log(level, msg, file, line);
                 }
             }
diff --git a/rosrust/tests/can_throttle_logs.rs b/rosrust/tests/can_throttle_logs.rs
index bcda36f..3f6e881 100644
--- a/rosrust/tests/can_throttle_logs.rs
+++ b/rosrust/tests/can_throttle_logs.rs
@@ -20,24 +20,33 @@ fn can_throttle_logs() {
         })
         .unwrap();
 
+    // Wait for the pub/sub connection to be established
+    rosrust::sleep(rosrust::Duration::from_seconds(1));
+
     let mut received_messages_counter = 0;
 
-    let rate = rosrust::rate(1.0);
-    for _ in 0..10 {
-        for item in rx.try_iter() {
-            println!("Received message at level {}: {}", item.0, item.1);
-            received_messages_counter += 1;
+    let rate = rosrust::rate(10.0);
+    let start_time = rosrust::now();
+    for i in 0..11 {
+        // Add a gap between t = 0.3s and t = 0.7s
+        // Logs should be produced at:
+        // t=0s, t=0.2s, t=0.7s, t=0.9s
+        if i < 4 || i > 6 {
+            rosrust::ros_info_throttle!(0.2, "[{}] info message", rosrust::now() - start_time);
         }
-
-        rosrust::ros_info_throttle!(5.0, "info message");
         rate.sleep();
     }
 
+    for item in rx.try_iter() {
+        println!("Received message at level {}: {}", item.0, item.1);
+        received_messages_counter += 1;
+    }
+
     if received_messages_counter == 0 {
         panic!("Failed to receive data on /rosout_agg");
-    } else if received_messages_counter > 2 {
+    } else if received_messages_counter != 4 {
         panic!(
-            "Received {} messages, not throttled",
+            "Received {} messages, not throttled properly",
             received_messages_counter
         );
     }