diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 3f1da26d1d..3b5b22515a 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 @@ class Clock 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 @@ -133,9 +138,10 @@ class Clock bool before_jump, void * user_data); - /// Internal storage backed by rcl - std::shared_ptr rcl_clock_; - rcl_allocator_t allocator_; + /// Private internal storage + class Impl; + + std::shared_ptr impl_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 5a7fb8ae7c..4993df7f63 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" @@ -23,6 +24,31 @@ namespace rclcpp { +class Clock::Impl +{ +public: + explicit Impl(rcl_clock_type_t clock_type) + : allocator_{rcl_get_default_allocator()} + { + rcl_ret_t ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_); + if (ret != RCL_RET_OK) { + exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); + } + } + + ~Impl() + { + rcl_ret_t ret = rcl_clock_fini(&rcl_clock_); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR("Failed to fini rcl clock."); + } + } + + rcl_clock_t rcl_clock_; + rcl_allocator_t allocator_; + std::mutex clock_mutex_; +}; + JumpHandler::JumpHandler( pre_callback_t pre_callback, post_callback_t post_callback, @@ -32,35 +58,17 @@ JumpHandler::JumpHandler( notice_threshold(threshold) {} -static -void -rcl_clock_deleter(rcl_clock_t * rcl_clock) -{ - auto ret = rcl_clock_fini(rcl_clock); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to fini rcl clock."); - } - delete rcl_clock; -} - Clock::Clock(rcl_clock_type_t clock_type) -{ - allocator_ = rcl_get_default_allocator(); - rcl_clock_ = std::shared_ptr(new rcl_clock_t(), rcl_clock_deleter); - auto ret = rcl_clock_init(clock_type, rcl_clock_.get(), &allocator_); - if (ret != RCL_RET_OK) { - exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); - } -} +: impl_(new Clock::Impl(clock_type)) {} Clock::~Clock() {} Time Clock::now() { - Time now(0, 0, rcl_clock_->type); + Time now(0, 0, impl_->rcl_clock_.type); - auto ret = rcl_clock_get_now(rcl_clock_.get(), &now.rcl_time_.nanoseconds); + auto ret = rcl_clock_get_now(&impl_->rcl_clock_, &now.rcl_time_.nanoseconds); if (ret != RCL_RET_OK) { exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); } @@ -71,13 +79,13 @@ Clock::now() bool Clock::ros_time_is_active() { - if (!rcl_clock_valid(rcl_clock_.get())) { + if (!rcl_clock_valid(&impl_->rcl_clock_)) { RCUTILS_LOG_ERROR("ROS time not valid!"); return false; } bool is_enabled = false; - auto ret = rcl_is_enabled_ros_time_override(rcl_clock_.get(), &is_enabled); + auto ret = rcl_is_enabled_ros_time_override(&impl_->rcl_clock_, &is_enabled); if (ret != RCL_RET_OK) { exceptions::throw_from_rcl_error( ret, "Failed to check ros_time_override_status"); @@ -88,13 +96,19 @@ Clock::ros_time_is_active() rcl_clock_t * Clock::get_clock_handle() noexcept { - return rcl_clock_.get(); + return &impl_->rcl_clock_; } rcl_clock_type_t Clock::get_clock_type() const noexcept { - return rcl_clock_->type; + return impl_->rcl_clock_.type; +} + +std::mutex & +Clock::get_clock_mutex() noexcept +{ + return impl_->clock_mutex_; } void @@ -126,22 +140,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_.get(), 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::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"); + } } - std::weak_ptr weak_clock = rcl_clock_; + std::weak_ptr weak_impl = impl_; // *INDENT-OFF* // create shared_ptr that removes the callback automatically when all copies are destructed - return JumpHandler::SharedPtr(handler.release(), [weak_clock](JumpHandler * handler) noexcept { - auto shared_clock = weak_clock.lock(); - if (shared_clock) { - rcl_ret_t ret = rcl_clock_remove_jump_callback(shared_clock.get(), Clock::on_time_jump, - handler); + 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) { RCUTILS_LOG_ERROR("Failed to remove time jump callback"); } diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 8291a12f5d..6f3259dd64 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(); + } } }