Fix log throttling

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);
                 }
             }

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
         );
     }