Ensure all rclcpp::Clock accesses are thread-safe.

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

rclcpp/include/rclcpp/clock.hpp

@@ -17,6 +17,7 @@
 
 #include <functional>
 #include <memory>
+#include <mutex>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
@@ -96,6 +97,10 @@ public:
   rcl_clock_type_t
   get_clock_type() const noexcept;
 
+  RCLCPP_PUBLIC
+  std::mutex &
+  get_clock_mutex() noexcept;
+
   // Add a callback to invoke if the jump threshold is exceeded.
   /**
    * These callback functions must remain valid as long as the

rclcpp/src/rclcpp/clock.cpp

@@ -15,6 +15,7 @@
 #include "rclcpp/clock.hpp"
 
 #include <memory>
+#include <thread>
 
 #include "rclcpp/exceptions.hpp"
 
@@ -45,6 +46,7 @@ public:
 
   rcl_clock_t rcl_clock_;
   rcl_allocator_t allocator_;
+  std::mutex clock_mutex_;
 };
 
 JumpHandler::JumpHandler(
@@ -103,6 +105,12 @@ Clock::get_clock_type() const noexcept
   return impl_->rcl_clock_.type;
 }
 
+std::mutex &
+Clock::get_clock_mutex() noexcept
+{
+  return impl_->clock_mutex_;
+}
+
 void
 Clock::on_time_jump(
   const struct rcl_time_jump_t * time_jump,
@@ -132,12 +140,15 @@ Clock::create_jump_callback(
     throw std::bad_alloc{};
   }
 
+  {
+    std::lock_guard<std::mutex> clock_guard(impl_->clock_mutex_);
     // Try to add the jump callback to the clock
-  rcl_ret_t ret = rcl_clock_add_jump_callback(
-    &impl_->rcl_clock_, threshold, Clock::on_time_jump,
-    handler.get());
-  if (RCL_RET_OK != ret) {
-    exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+    rcl_ret_t ret = rcl_clock_add_jump_callback(
+      &impl_->rcl_clock_, threshold, Clock::on_time_jump,
+      handler.get());
+    if (RCL_RET_OK != ret) {
+      exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+    }
   }
 
   std::weak_ptr<Clock::Impl> weak_impl = impl_;
@@ -146,6 +157,7 @@ Clock::create_jump_callback(
   return JumpHandler::SharedPtr(handler.release(), [weak_impl](JumpHandler * handler) noexcept {
     auto shared_impl = weak_impl.lock();
     if (shared_impl) {
+      std::lock_guard<std::mutex> clock_guard(shared_impl->clock_mutex_);
       rcl_ret_t ret = rcl_clock_remove_jump_callback(&shared_impl->rcl_clock_,
           Clock::on_time_jump, handler);
       if (RCL_RET_OK != ret) {

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
@@ -55,15 +59,18 @@ 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());
+    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();
+    }
   }
 }