diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 02c6167..3f1da26 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -16,6 +16,7 @@ #define RCLCPP__CLOCK_HPP_ #include +#include #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" @@ -133,8 +134,7 @@ private: void * user_data); /// Internal storage backed by rcl - rcl_clock_t rcl_clock_; - friend TimeSource; /// Allow TimeSource to access the rcl_clock_ datatype. + std::shared_ptr rcl_clock_; rcl_allocator_t allocator_; }; diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index a0e02cc..5a7fb8a 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,6 +14,8 @@ #include "rclcpp/clock.hpp" +#include + #include "rclcpp/exceptions.hpp" #include "rcutils/logging_macros.h" @@ -30,29 +32,35 @@ 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(); - auto ret = rcl_clock_init(clock_type, &rcl_clock_, &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"); } } -Clock::~Clock() -{ - auto ret = rcl_clock_fini(&rcl_clock_); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to fini rcl clock."); - } -} +Clock::~Clock() {} Time Clock::now() { - Time now(0, 0, rcl_clock_.type); + Time now(0, 0, rcl_clock_->type); - auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds); + auto ret = rcl_clock_get_now(rcl_clock_.get(), &now.rcl_time_.nanoseconds); if (ret != RCL_RET_OK) { exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); } @@ -63,13 +71,13 @@ Clock::now() bool Clock::ros_time_is_active() { - if (!rcl_clock_valid(&rcl_clock_)) { + if (!rcl_clock_valid(rcl_clock_.get())) { RCUTILS_LOG_ERROR("ROS time not valid!"); return false; } bool is_enabled = false; - auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled); + auto ret = rcl_is_enabled_ros_time_override(rcl_clock_.get(), &is_enabled); if (ret != RCL_RET_OK) { exceptions::throw_from_rcl_error( ret, "Failed to check ros_time_override_status"); @@ -80,13 +88,13 @@ Clock::ros_time_is_active() rcl_clock_t * Clock::get_clock_handle() noexcept { - return &rcl_clock_; + return rcl_clock_.get(); } rcl_clock_type_t Clock::get_clock_type() const noexcept { - return rcl_clock_.type; + return rcl_clock_->type; } void @@ -120,23 +128,25 @@ Clock::create_jump_callback( // 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, + 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::weak_ptr weak_clock = rcl_clock_; // *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 { - rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump, - handler); - delete handler; - handler = NULL; - if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR("Failed to remove time jump callback"); + 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); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR("Failed to remove time jump callback"); + } } + delete handler; }); // *INDENT-ON* } diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index ccf05bf..fd1da9c 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -192,7 +192,7 @@ void TimeSource::set_clock( enable_ros_time(clock); } - auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds()); + auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds()); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error( ret, "Failed to set ros_time_override_status"); @@ -275,7 +275,7 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S void TimeSource::enable_ros_time(std::shared_ptr clock) { - auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_); + auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error( ret, "Failed to enable ros_time_override_status"); @@ -284,7 +284,7 @@ void TimeSource::enable_ros_time(std::shared_ptr clock) void TimeSource::disable_ros_time(std::shared_ptr clock) { - auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_); + auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error( ret, "Failed to enable ros_time_override_status"); diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp index 773819a..830c031 100644 --- a/rclcpp/test/test_time.cpp +++ b/rclcpp/test/test_time.cpp @@ -56,6 +56,18 @@ TEST(TestTime, clock_type_access) { EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type()); } +// Check that the clock may go out of the scope before the jump callback without leading in UB. +TEST(TestTime, clock_jump_callback_destruction_order) { + rclcpp::JumpHandler::SharedPtr handler; + { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + rcl_jump_threshold_t threshold; + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + handler = ros_clock.create_jump_callback([]() {}, [](const rcl_time_jump_t &) {}, threshold); + } +} + TEST(TestTime, time_sources) { using builtin_interfaces::msg::Time; rclcpp::Clock ros_clock(RCL_ROS_TIME);