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/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();
+    }
   }
 }