diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index fa35046..2bcd257 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -31,53 +31,18 @@ namespace rclcpp class TimeSource; -/// A struct to represent a timejump. -/** - * It represents the time jump duration and whether it changed clock type. - */ -struct TimeJump -{ - typedef enum ClockChange_t - { - ROS_TIME_NO_CHANGE, ///< The time type before and after the jump is ROS_TIME - ROS_TIME_ACTIVATED, ///< The time type switched to ROS_TIME from SYSTEM_TIME - ROS_TIME_DEACTIVATED, ///< The time type switched to SYSTEM_TIME from ROS_TIME - SYSTEM_TIME_NO_CHANGE ///< The time type before and after the jump is SYSTEM_TIME - } ClockChange_t; - - ClockChange_t jump_type_; ///< The change in clock_type if there is one. - rcl_duration_t delta_; ///< The change in time value. -}; - -/// A class to store a threshold for a TimeJump. -/** - * This class can be used to evaluate a time jump's magnitude. - */ -class JumpThreshold -{ -public: - uint64_t min_forward_; ///< The minimum jump forward to be considered exceeded.. - uint64_t min_backward_; ///< The minimum backwards jump to be considered exceeded. - bool on_clock_change_; ///< Whether to trigger on any clock type change. - - // Test if the threshold is exceeded by a TimeJump - RCLCPP_PUBLIC - bool - is_exceeded(const TimeJump & jump); -}; - class JumpHandler { public: RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler) JumpHandler( std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold); + std::function post_callback, + const rcl_jump_threshold_t & threshold); std::function pre_callback; - std::function post_callback; - JumpThreshold notice_threshold; + std::function post_callback; + rcl_jump_threshold_t notice_threshold; }; class Clock @@ -116,32 +81,22 @@ public: JumpHandler::SharedPtr create_jump_callback( std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold); + std::function post_callback, + const rcl_jump_threshold_t & threshold); private: - // A method for TimeSource to get a list of callbacks to invoke while updating + // Invoke time jump callback RCLCPP_PUBLIC - std::vector - get_triggered_callback_handlers(const TimeJump & jump); - - // Invoke callbacks that are valid and outside threshold. - RCLCPP_PUBLIC - static void invoke_prejump_callbacks( - const std::vector & callbacks); - - RCLCPP_PUBLIC - static void invoke_postjump_callbacks( - const std::vector & callbacks, - const TimeJump & jump); + static void + on_time_jump( + const struct rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data); /// Internal storage backed by rcl rcl_clock_t rcl_clock_; friend TimeSource; /// Allow TimeSource to access the rcl_clock_ datatype. rcl_allocator_t allocator_; - - std::mutex callback_list_mutex_; - std::vector> active_jump_handlers_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index df38ec0..86015fe 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -28,27 +28,11 @@ namespace rclcpp { -bool -JumpThreshold::is_exceeded(const TimeJump & jump) -{ - if (on_clock_change_ && - (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED || - jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED)) - { - return true; - } - if ((uint64_t)jump.delta_.nanoseconds > min_forward_ || - (uint64_t)jump.delta_.nanoseconds < min_backward_) - { - return true; - } - return false; -} JumpHandler::JumpHandler( std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold) + std::function post_callback, + const rcl_jump_threshold_t & threshold) : pre_callback(pre_callback), post_callback(post_callback), notice_threshold(threshold) @@ -115,66 +99,51 @@ Clock::get_clock_type() return rcl_clock_.type; } +void +Clock::on_time_jump( + const struct rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data) +{ + rclcpp::JumpHandler * handler = static_cast(user_data); + if (before_jump && handler->pre_callback) { + handler->pre_callback(); + } else if (!before_jump && handler->post_callback) { + handler->post_callback(*time_jump); + } +} + rclcpp::JumpHandler::SharedPtr Clock::create_jump_callback( std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold) + std::function post_callback, + const rcl_jump_threshold_t & threshold) { - // JumpHandler jump_callback; - auto jump_callback = - std::make_shared(pre_callback, post_callback, threshold); - { - std::lock_guard guard(callback_list_mutex_); - active_jump_handlers_.push_back(jump_callback); + // Allocate a new jump handler + auto handler = new rclcpp::JumpHandler(pre_callback, post_callback, threshold); + if (nullptr == handler) { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, "Failed to allocate jump handler"); } - return jump_callback; -} -std::vector -Clock::get_triggered_callback_handlers(const TimeJump & jump) -{ - std::vector callbacks; - std::lock_guard guard(callback_list_mutex_); - active_jump_handlers_.erase( - std::remove_if( - active_jump_handlers_.begin(), - active_jump_handlers_.end(), - [&callbacks, &jump](const std::weak_ptr & wjcb) { - if (auto jcb = wjcb.lock()) { - if (jcb->notice_threshold.is_exceeded(jump)) { - callbacks.push_back(jcb); - } - return false; - } - // Lock failed so clear the weak pointer. - return true; - }), - active_jump_handlers_.end()); - return callbacks; -} + // Try to add the jump callback to the clock + rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold, + rclcpp::Clock::on_time_jump, handler); + if (RCL_RET_OK != ret) { + delete handler; + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback"); + } -void -Clock::invoke_prejump_callbacks( - const std::vector & callbacks) -{ - for (const auto cb : callbacks) { - if (cb->pre_callback != nullptr) { - cb->pre_callback(); + // *INDENT-OFF* + // create shared_ptr that removes the callback automatically when all copies are destructed + return rclcpp::JumpHandler::SharedPtr(handler, [this](rclcpp::JumpHandler * handler) noexcept { + rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, rclcpp::Clock::on_time_jump, + handler); + delete handler; + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR("Failed to remove time jump callback"); } - } -} - -void -Clock::invoke_postjump_callbacks( - const std::vector & callbacks, - const TimeJump & jump) -{ - for (auto cb : callbacks) { - if (cb->post_callback != nullptr) { - cb->post_callback(jump); - } - } + }); + // *INDENT-ON* } } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index bfd0ea2..a18400a 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -147,54 +147,18 @@ void TimeSource::set_clock( const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, std::shared_ptr clock) { - // Compute diff - rclcpp::Time msg_time = rclcpp::Time(*msg); - rclcpp::Time now = clock->now(); - auto diff = now - msg_time; - rclcpp::TimeJump jump; - jump.delta_.nanoseconds = diff.nanoseconds(); - - // Compute jump type - if (clock->ros_time_is_active()) { - if (set_ros_time_enabled) { - jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE; - } else { - jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED; - } - } else if (!clock->ros_time_is_active()) { - if (set_ros_time_enabled) { - jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_ACTIVATED; - } else { - jump.jump_type_ = TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE; - } - } - - if (jump.jump_type_ == TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE) { - // No change/no updates don't act. - return; - } - - auto active_callbacks = clock->get_triggered_callback_handlers(jump); - clock->invoke_prejump_callbacks(active_callbacks); - // Do change - if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED) { + if (!set_ros_time_enabled && clock->ros_time_is_active()) { disable_ros_time(clock); - } else if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED) { + } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { enable_ros_time(clock); } - if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED || - jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE) - { - auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), msg_time.nanoseconds()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to set ros_time_override_status"); - } + auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to set ros_time_override_status"); } - // Post change callbacks - clock->invoke_postjump_callbacks(active_callbacks, jump); } void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg) diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index cd62974..bc51820 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -133,27 +133,30 @@ TEST_F(TestTimeSource, clock) { class CallbackObject { public: - CallbackObject() - : last_precallback_id_(0), - last_postcallback_id_(0) - {} - int last_precallback_id_; - void pre_callback(int id) {last_precallback_id_ = id;} + int pre_callback_calls_ = 0; + int last_precallback_id_ = 0; + void pre_callback(int id) + { + last_precallback_id_ = id; + ++pre_callback_calls_; + } - int last_postcallback_id_; - rclcpp::TimeJump last_timejump_; - void post_callback(const rclcpp::TimeJump & jump, int id) + int post_callback_calls_ = 0; + int last_postcallback_id_ = 0; + rcl_time_jump_t last_timejump_; + void post_callback(const rcl_time_jump_t & jump, int id) { last_postcallback_id_ = id; last_timejump_ = jump; + ++post_callback_calls_; } }; TEST_F(TestTimeSource, callbacks) { CallbackObject cbo; - rclcpp::JumpThreshold jump_threshold; - jump_threshold.min_forward_ = 0; - jump_threshold.min_backward_ = 0; - jump_threshold.on_clock_change_ = true; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); @@ -212,7 +215,7 @@ TEST_F(TestTimeSource, callbacks) { // Register a callback handler with only pre_callback rclcpp::JumpHandler::SharedPtr callback_handler3 = ros_clock->create_jump_callback( std::bind(&CallbackObject::pre_callback, &cbo, 3), - std::function(), + std::function(), jump_threshold); trigger_clock_changes(node); @@ -233,10 +236,10 @@ TEST_F(TestTimeSource, callbacks) { TEST_F(TestTimeSource, callback_handler_erasure) { CallbackObject cbo; - rclcpp::JumpThreshold jump_threshold; - jump_threshold.min_forward_ = 0; - jump_threshold.min_backward_ = 0; - jump_threshold.on_clock_change_ = true; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); @@ -356,3 +359,44 @@ TEST_F(TestTimeSource, parameter_activation) { rclcpp::spin_some(node); EXPECT_FALSE(ros_clock->ros_time_is_active()); } + +TEST_F(TestTimeSource, no_pre_jump_callback) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + nullptr, + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + ASSERT_EQ(0, cbo.last_precallback_id_); + ASSERT_EQ(0, cbo.last_postcallback_id_); + ts.attachClock(ros_clock); + + // Activate ROS time + auto parameters_client = std::make_shared(node); + ASSERT_TRUE(parameters_client->wait_for_service(2s)); + auto set_parameters_results = parameters_client->set_parameters({ + rclcpp::Parameter("use_sim_time", true) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + // SyncParametersClient returns when parameters have been set on the node_parameters interface, + // but it doesn't mean the on_parameter_event subscription in TimeSource has been called. + // Spin some to handle that subscription. + rclcpp::spin_some(node); + ASSERT_TRUE(ros_clock->ros_time_is_active()); + + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.pre_callback_calls_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + EXPECT_EQ(1, cbo.post_callback_calls_); +}