From f9c4894f96ea083fc5acfcea4f1ea83850759e63 Mon Sep 17 00:00:00 2001
From: jmachowinski <jmachowinski@users.noreply.github.com>
Date: Tue, 2 Apr 2024 18:11:30 +0200
Subject: [PATCH] Flaky timer test fix (#2469)

* fix(time_source): Fixed possible race condition

Signed-off-by: Janosch Machowinski <j.machowinski@nospam.org>

* fix(test_executors_time_cancel_behaviour): Fixed multiple race conditions

Signed-off-by: Janosch Machowinski <j.machowinski@nospam.org>

---------

Signed-off-by: Janosch Machowinski <j.machowinski@nospam.org>
Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
---
 rclcpp/src/rclcpp/time_source.cpp             | 13 +++--
 .../test_executors_timer_cancel_behavior.cpp  | 50 ++++++++++++++++---
 2 files changed, 52 insertions(+), 11 deletions(-)

diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp
index 465ceaf5a7..c6e39ace70 100644
--- a/rclcpp/src/rclcpp/time_source.cpp
+++ b/rclcpp/src/rclcpp/time_source.cpp
@@ -288,10 +288,10 @@ class TimeSource::NodeState final
   // Detach the attached node
   void detachNode()
   {
-    std::lock_guard<std::mutex> guard(node_base_lock_);
     // destroy_clock_sub() *must* be first here, to ensure that the executor
     // can't possibly call any of the callbacks as we are cleaning up.
     destroy_clock_sub();
+    std::lock_guard<std::mutex> guard(node_base_lock_);
     clocks_state_.disable_ros_time();
     if (on_set_parameters_callback_) {
       node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get());
@@ -409,9 +409,14 @@ class TimeSource::NodeState final
       "/clock",
       qos_,
       [this](std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
-        // We are using node_base_ as an indication if there is a node attached.
-        // Only call the clock_cb if that is the case.
-        if (node_base_ != nullptr) {
+        bool execute_cb = false;
+        {
+          std::lock_guard<std::mutex> guard(node_base_lock_);
+          // We are using node_base_ as an indication if there is a node attached.
+          // Only call the clock_cb if that is the case.
+          execute_cb = node_base_ != nullptr;
+        }
+        if (execute_cb) {
           clock_cb(msg);
         }
       },
diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp
index 7fd1676c5d..f6bbd2ccc8 100644
--- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp
@@ -54,8 +54,16 @@ class TimerNode : public rclcpp::Node
       std::bind(&TimerNode::Timer2Callback, this));
   }
 
-  int GetTimer1Cnt() {return cnt1_;}
-  int GetTimer2Cnt() {return cnt2_;}
+  int GetTimer1Cnt()
+  {
+    const std::lock_guard<std::mutex> lock(mutex_);
+    return cnt1_;
+  }
+  int GetTimer2Cnt()
+  {
+    const std::lock_guard<std::mutex> lock(mutex_);
+    return cnt2_;
+  }
 
   void ResetTimer1()
   {
@@ -82,16 +90,24 @@ class TimerNode : public rclcpp::Node
 private:
   void Timer1Callback()
   {
-    cnt1_++;
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      cnt1_++;
+    }
     RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_);
   }
 
   void Timer2Callback()
   {
-    cnt2_++;
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      cnt2_++;
+    }
     RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_);
   }
 
+  std::mutex mutex_;
+
   rclcpp::TimerBase::SharedPtr timer1_;
   rclcpp::TimerBase::SharedPtr timer2_;
   int cnt1_ = 0;
@@ -130,6 +146,18 @@ class ClockPublisher : public rclcpp::Node
     }
   }
 
+  bool wait_for_connection(std::chrono::nanoseconds timeout)
+  {
+    auto end_time = std::chrono::steady_clock::now() + timeout;
+    while (clock_publisher_->get_subscription_count() == 0 &&
+      (std::chrono::steady_clock::now() < end_time))
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+    return clock_publisher_->get_subscription_count() != 0;
+  }
+
   void sleep_for(rclcpp::Duration duration)
   {
     rclcpp::Time start_time(0, 0, RCL_ROS_TIME);
@@ -148,7 +176,10 @@ class ClockPublisher : public rclcpp::Node
         return;
       }
       std::this_thread::sleep_for(realtime_clock_step_.to_chrono<std::chrono::milliseconds>());
-      rostime_ += ros_update_duration_;
+      {
+        const std::lock_guard<std::mutex> lock(mutex_);
+        rostime_ += ros_update_duration_;
+      }
     }
   }
 
@@ -163,9 +194,11 @@ class ClockPublisher : public rclcpp::Node
 
   void PublishClock()
   {
-    const std::lock_guard<std::mutex> lock(mutex_);
     auto message = rosgraph_msgs::msg::Clock();
-    message.clock = rostime_;
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      message.clock = rostime_;
+    }
     clock_publisher_->publish(message);
   }
 
@@ -227,6 +260,9 @@ class TestTimerCancelBehavior : public ::testing::Test
       [this]() {
         executor.spin();
       });
+
+    EXPECT_TRUE(this->sim_clock_node->wait_for_connection(50ms));
+    EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->ros_time_is_active());
   }
 
   void TearDown()