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,6 +140,8 @@ 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,
@@ -139,6 +149,7 @@ Clock::create_jump_callback(
     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_;
   // *INDENT-OFF*
@@ -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,12 +41,15 @@ TimerBase::TimerBase(
   timer_handle_ = std::shared_ptr<rcl_timer_t>(
     new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable
     {
+      {
+        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
       clock.reset();
@@ -55,6 +59,8 @@ TimerBase::TimerBase(
   *timer_handle_.get() = rcl_get_zero_initialized_timer();
 
   rcl_clock_t * clock_handle = clock_->get_clock_handle();
+  {
+    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,
@@ -65,6 +71,7 @@ TimerBase::TimerBase(
         "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
       rcl_reset_error();
     }
+  }
 }
 
 TimerBase::~TimerBase()