diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 3f1da26..a479829 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -133,9 +133,10 @@ private: 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 5a7fb8a..81395d2 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -23,6 +23,30 @@ 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_; +}; + JumpHandler::JumpHandler( pre_callback_t pre_callback, post_callback_t post_callback, @@ -32,35 +56,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 +77,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 +94,13 @@ 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; } void @@ -126,22 +132,22 @@ Clock::create_jump_callback( throw std::bad_alloc{}; } - // Try to add the jump callback to the clock + // 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, + &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) { + 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"); }