Ensure rclcpp::TimerBase thread-safety.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rclcpp/include/rclcpp/clock.hpp

@@ -16,6 +16,8 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
+#include <thread>
+#include <mutex>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
@@ -95,6 +97,12 @@ public:
   rcl_clock_type_t
   get_clock_type() const noexcept;
 
+  RCLCPP_PUBLIC
+  std::mutex &
+  get_clock_mutex() noexcept {
+    return clock_mutex_;
+  }
+
   // Add a callback to invoke if the jump threshold is exceeded.
   /**
    * These callback functions must remain valid as long as the
@@ -136,6 +144,7 @@ private:
   rcl_clock_t rcl_clock_;
   friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
   rcl_allocator_t allocator_;
+  std::mutex clock_mutex_;
 };
 
 }  // namespace rclcpp

rclcpp/resource/logging.hpp.em

@@ -128,9 +128,9 @@ def get_rclcpp_suffix_from_features(features):
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \
-    auto get_time_point = [&clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
+    auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
       try { \
-        *time_point = clock.now().nanoseconds(); \
+        *time_point = c.now().nanoseconds(); \
       } catch (...) { \
         RCUTILS_SAFE_FWRITE_TO_STDERR( \
         "[rclcpp|logging.hpp] RCLCPP_@(severity)@(suffix) could not get current time stamp\n"); \

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -402,6 +402,8 @@ NodeParameters::declare_parameter(
 
   // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
   if (nullptr != events_publisher_) {
+    parameter_event.node = combined_name_;
+    parameter_event.stamp = node_clock_->get_clock()->now();
     events_publisher_->publish(parameter_event);
   }
 

rclcpp/src/rclcpp/timer.cpp

@@ -17,6 +17,7 @@
 #include <chrono>
 #include <string>
 #include <memory>
+#include <thread>
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
@@ -40,11 +41,14 @@ TimerBase::TimerBase(
   timer_handle_ = std::shared_ptr<rcl_timer_t>(
     new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable
     {
-      if (rcl_timer_fini(timer) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
-        rcl_reset_error();
+      {
+        std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+        if (rcl_timer_fini(timer) != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "rclcpp",
+            "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
+          rcl_reset_error();
+        }
       }
       delete timer;
       // Captured shared pointers by copy, reset to make sure timer is finalized before clock
@@ -54,16 +58,19 @@ TimerBase::TimerBase(
 
   *timer_handle_.get() = rcl_get_zero_initialized_timer();
 
-  rcl_clock_t * clock_handle = clock_->get_clock_handle();
-  if (
-    rcl_timer_init(
-      timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
-      rcl_get_default_allocator()) != RCL_RET_OK)
   {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
-    rcl_reset_error();
+    std::lock_guard<std::mutex> clock_guard(clock_->get_clock_mutex());
+    rcl_clock_t * clock_handle = clock_->get_clock_handle();
+    if (
+      rcl_timer_init(
+        timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
+        rcl_get_default_allocator()) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
   }
 }
 

rclcpp/test/test_logging.cpp

@@ -77,16 +77,33 @@ public:
   }
 };
 
+class DummyNode
+{
+public:
+  DummyNode()
+  {
+    clock_ = rclcpp::Clock::make_shared(RCL_ROS_TIME);
+  }
+  rclcpp::Clock::SharedPtr get_clock()
+  {
+    return clock_;
+  }
+
+private:
+  rclcpp::Clock::SharedPtr clock_;
+};
+
 TEST_F(TestLoggingMacros, test_logging_named) {
   for (int i : {1, 2, 3}) {
     RCLCPP_DEBUG(g_logger, "message %d", i);
   }
+  size_t expected_location = __LINE__ - 2u;
   EXPECT_EQ(3u, g_log_calls);
   EXPECT_TRUE(g_last_log_event.location != NULL);
   if (g_last_log_event.location) {
     EXPECT_STREQ("TestBody", g_last_log_event.location->function_name);
     EXPECT_THAT(g_last_log_event.location->file_name, EndsWith("test_logging.cpp"));
-    EXPECT_EQ(82u, g_last_log_event.location->line_number);
+    EXPECT_EQ(expected_location, g_last_log_event.location->line_number);
   }
   EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, g_last_log_event.level);
   EXPECT_EQ("name", g_last_log_event.name);
@@ -210,6 +227,26 @@ TEST_F(TestLoggingMacros, test_throttle) {
       EXPECT_EQ(6u, g_log_calls);
     }
   }
+  DummyNode node;
+  rcl_clock_t * node_clock = node.get_clock()->get_clock_handle();
+  ASSERT_TRUE(node_clock);
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(node_clock));
+  EXPECT_EQ(6u, g_log_calls);
+  EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, RCUTILS_MS_TO_NS(10)));
+  for (uint64_t i = 0; i < 3; ++i) {
+    RCLCPP_DEBUG_THROTTLE(g_logger, *node.get_clock(), 10, "Throttling");
+    if (i == 0) {
+      EXPECT_EQ(7u, g_log_calls);
+      rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5);
+      EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns));
+    } else if (i == 1) {
+      EXPECT_EQ(7u, g_log_calls);
+      rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5);
+      EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns));
+    } else {
+      EXPECT_EQ(8u, g_log_calls);
+    }
+  }
 }
 
 bool log_function(rclcpp::Logger logger)