diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index a479829..3b5b225 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -17,6 +17,7 @@ #include #include +#include #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 diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 81395d2..4993df7 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -15,6 +15,7 @@ #include "rclcpp/clock.hpp" #include +#include #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 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 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 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) { diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 8291a12..6f3259d 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" @@ -40,11 +41,14 @@ TimerBase::TimerBase( timer_handle_ = std::shared_ptr( 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 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 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(); + } } }