[eloquent] Ensure rcl_clock accesses are thread-safe (ABI-safe version).

rclcpp/include/rclcpp/clock.hpp

@@ -16,6 +16,9 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
+#include <mutex>
+#include <shared_mutex>  // NOLINT
+#include <unordered_map>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
@@ -138,6 +141,19 @@ class Clock
   rcl_allocator_t allocator_;
 };
 
+// This code is an ABI-compatible version of the code that went in for
+// https://github.com/ros2/rclcpp/pull/999.  The way that this works is to have
+// a global map of pointers to mutexes, one per clock class that is created.
+// During the rclcpp::Clock constructor, it adds a new one to this map, and
+// during the rclcpp::Clock destructor, it removes it from this map.  When
+// it needs to lock it, it just looks it up in the map.  The map itself has
+// to be protected when adding a new mutex, removing one, or looking it up,
+// hence the g_clock_map_mutex.
+extern std::shared_timed_mutex g_clock_map_mutex;
+extern std::unordered_map<rclcpp::Clock *, std::mutex> g_clock_mutex_map;
+
+std::mutex & get_clock_mutex(rclcpp::Clock * clock);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CLOCK_HPP_

rclcpp/src/rclcpp/clock.cpp

@@ -12,6 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <mutex>
+#include <shared_mutex>  // NOLINT
+#include <unordered_map>
+
 #include "rclcpp/clock.hpp"
 
 #include "rclcpp/exceptions.hpp"
@@ -21,6 +25,15 @@
 namespace rclcpp
 {
 
+std::shared_timed_mutex g_clock_map_mutex;
+std::unordered_map<rclcpp::Clock *, std::mutex> g_clock_mutex_map;
+
+std::mutex & get_clock_mutex(rclcpp::Clock * clock)
+{
+  std::shared_lock<std::shared_timed_mutex> lk(g_clock_map_mutex);
+  return g_clock_mutex_map.at(clock);
+}
+
 JumpHandler::JumpHandler(
   pre_callback_t pre_callback,
   post_callback_t post_callback,
@@ -37,10 +50,22 @@ Clock::Clock(rcl_clock_type_t clock_type)
   if (ret != RCL_RET_OK) {
     exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
   }
+
+  {
+    std::lock_guard<std::shared_timed_mutex> lg(g_clock_map_mutex);
+    // Add a new default-constructed mutex, keyed off of this pointer.
+    g_clock_mutex_map[this];
+  }
 }
 
 Clock::~Clock()
 {
+  {
+    std::lock_guard<std::shared_timed_mutex> lg(g_clock_map_mutex);
+    // Remove the mutex corresponding to this clock object.
+    g_clock_mutex_map.erase(this);
+  }
+
   auto ret = rcl_clock_fini(&rcl_clock_);
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
@@ -118,18 +143,26 @@ Clock::create_jump_callback(
     throw std::bad_alloc{};
   }
 
-  // Try to add the jump callback to the clock
-  rcl_ret_t ret = rcl_clock_add_jump_callback(
-    &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");
+  // Lookup the per-object mutex
+  std::mutex & clock_mutex = get_clock_mutex(this);
+  {
+    std::lock_guard<std::mutex> clock_guard(clock_mutex);
+    // Try to add the jump callback to the clock
+    rcl_ret_t ret = rcl_clock_add_jump_callback(
+      &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");
+    }
   }
 
   // *INDENT-OFF*
   // create shared_ptr that removes the callback automatically when all copies are destructed
   // TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler
   return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept {
+    std::mutex & clock_mutex = get_clock_mutex(this);
+    std::lock_guard<std::mutex> clock_guard(clock_mutex);
+
     rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump,
         handler);
     delete handler;

rclcpp/src/rclcpp/timer.cpp

@@ -17,7 +17,9 @@
 #include <chrono>
 #include <string>
 #include <memory>
+#include <mutex>
 
+#include "rclcpp/clock.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
 
@@ -40,11 +42,16 @@ 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();
+      // Lookup the per-object mutex
+      std::mutex & clock_mutex = get_clock_mutex(this->clock_.get());
+      {
+        std::lock_guard<std::mutex> clock_guard(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 +62,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::mutex & clock_mutex = get_clock_mutex(this->clock_.get());
+    std::lock_guard<std::mutex> clock_guard(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();
+    }
   }
 }