From 634cb873a3106fa03a91bee89755184116abef40 Mon Sep 17 00:00:00 2001 From: jmachowinski Date: Fri, 12 Apr 2024 14:28:59 +0200 Subject: [PATCH] Added optional TimerInfo to timer callback (#2343) Signed-off-by: Alexis Tsogias Signed-off-by: Janosch Machowinski Co-authored-by: Alexis Tsogias Co-authored-by: Janosch Machowinski --- rclcpp/include/rclcpp/executor.hpp | 2 +- .../events_executor_event_types.hpp | 3 + .../rclcpp/experimental/timers_manager.hpp | 9 ++- .../strategies/allocator_memory_strategy.hpp | 4 +- rclcpp/include/rclcpp/timer.hpp | 57 ++++++++++++++----- rclcpp/src/rclcpp/executor.cpp | 12 ++-- .../static_single_threaded_executor.cpp | 12 ++-- .../events_executor/events_executor.cpp | 16 +++--- .../rclcpp/experimental/timers_manager.cpp | 29 +++++++--- .../rclcpp/executors/test_events_queue.cpp | 1 + .../node_interfaces/test_node_timers.cpp | 4 +- rclcpp/test/rclcpp/test_timer.cpp | 22 +++++++ 12 files changed, 126 insertions(+), 45 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index cfd7ea81fd..132b8150fe 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -487,7 +487,7 @@ class Executor */ RCLCPP_PUBLIC static void - execute_timer(rclcpp::TimerBase::SharedPtr timer); + execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & dataPtr); /// Run service server executable. /** diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp index 79c2c5f905..0da641ea6e 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ #define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ +#include + namespace rclcpp { namespace experimental @@ -34,6 +36,7 @@ enum ExecutorEventType struct ExecutorEvent { const void * entity_key; + std::shared_ptr data; int waitable_data; ExecutorEventType type; size_t num_events; diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index 197397e8b8..af3337bfd6 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -86,7 +86,8 @@ class TimersManager RCLCPP_PUBLIC TimersManager( std::shared_ptr context, - std::function on_ready_callback = nullptr); + std::function &)> on_ready_callback = nullptr); /** * @brief Destruct the TimersManager object making sure to stop thread and release memory. @@ -164,9 +165,10 @@ class TimersManager * the TimersManager on_ready_callback was passed during construction. * * @param timer_id the timer ID of the timer to execute + * @param data internal data of the timer */ RCLCPP_PUBLIC - void execute_ready_timer(const rclcpp::TimerBase * timer_id); + void execute_ready_timer(const rclcpp::TimerBase * timer_id, const std::shared_ptr & data); /** * @brief Get the amount of time before the next timer triggers. @@ -529,7 +531,8 @@ class TimersManager void execute_ready_timers_unsafe(); // Callback to be called when timer is ready - std::function on_ready_callback_; + std::function &)> on_ready_callback_ = nullptr; // Thread used to run the timers execution task std::thread timers_thread_; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1da372abfd..28eff94aed 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -370,7 +370,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy ++it; continue; } - if (!timer->call()) { + auto data = timer->call(); + if (!data) { // timer was cancelled, skip it. ++it; continue; @@ -379,6 +380,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy any_exec.timer = timer; any_exec.callback_group = group; any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); + any_exec.data = data; timer_handles_.erase(it); return; } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 6060d8bd78..ad9e96ea40 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,12 @@ namespace rclcpp { +struct TimerInfo +{ + Time expected_call_time; + Time actual_call_time; +}; + class TimerBase { public: @@ -101,16 +108,20 @@ class TimerBase * The multithreaded executor takes advantage of this to avoid scheduling * the callback multiple times. * - * \return `true` if the callback should be executed, `false` if the timer was canceled. + * \return a valid shared_ptr if the callback should be executed, + * an invalid shared_ptr (nullptr) if the timer was canceled. */ RCLCPP_PUBLIC - virtual bool + virtual std::shared_ptr call() = 0; /// Call the callback function when the timer signal is emitted. + /** + * \param[in] data the pointer returned by the call function + */ RCLCPP_PUBLIC virtual void - execute_callback() = 0; + execute_callback(const std::shared_ptr & data) = 0; RCLCPP_PUBLIC std::shared_ptr @@ -198,16 +209,17 @@ class TimerBase set_on_reset_callback(rcl_event_callback_t callback, const void * user_data); }; - using VoidCallbackType = std::function; using TimerCallbackType = std::function; +using TimerInfoCallbackType = std::function; /// Generic timer. Periodically executes a user-specified callback. template< typename FunctorT, typename std::enable_if< rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class GenericTimer : public TimerBase @@ -256,27 +268,28 @@ class GenericTimer : public TimerBase * \sa rclcpp::TimerBase::call * \throws std::runtime_error if it failed to notify timer that callback will occurr */ - bool + std::shared_ptr call() override { - rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); + auto timer_call_info_ = std::make_shared(); + rcl_ret_t ret = rcl_timer_call_with_info(timer_handle_.get(), timer_call_info_.get()); if (ret == RCL_RET_TIMER_CANCELED) { - return false; + return nullptr; } if (ret != RCL_RET_OK) { throw std::runtime_error("Failed to notify timer that callback occurred"); } - return true; + return timer_call_info_; } /** * \sa rclcpp::TimerBase::execute_callback */ void - execute_callback() override + execute_callback(const std::shared_ptr & data) override { TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); - execute_callback_delegate<>(); + execute_callback_delegate<>(*static_cast(data.get())); TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } @@ -288,7 +301,7 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(); } @@ -300,11 +313,26 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(*this); } + + template< + typename CallbackT = FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value + >::type * = nullptr + > + void + execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info_) + { + const TimerInfo info{Time{timer_call_info_.expected_call_time, clock_->get_clock_type()}, + Time{timer_call_info_.actual_call_time, clock_->get_clock_type()}}; + callback_(info); + } + /// Is the clock steady (i.e. is the time between ticks constant?) /** \return True if the clock used by this timer is steady. */ bool @@ -323,7 +351,8 @@ template< typename FunctorT, typename std::enable_if< rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class WallTimer : public GenericTimer diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a54d71e21b..0b78e12912 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -383,7 +383,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.timer->get_timer_handle().get())); - execute_timer(any_exec.timer); + execute_timer(any_exec.timer, any_exec.data); } if (any_exec.subscription) { TRACETOOLS_TRACEPOINT( @@ -547,9 +547,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } void -Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) +Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & dataPtr) { - timer->execute_callback(); + timer->execute_callback(dataPtr); } void @@ -690,6 +690,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) if (entity_iter != current_collection_.timers.end()) { auto callback_group = entity_iter->second.callback_group.lock(); if (callback_group && !callback_group->can_be_taken_from()) { + current_timer_index++; continue; } // At this point the timer is either ready for execution or was perhaps @@ -698,7 +699,9 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) // it from the wait result. wait_result_->clear_timer_with_index(current_timer_index); // Check that the timer should be called still, i.e. it wasn't canceled. - if (!timer->call()) { + any_executable.data = timer->call(); + if (!any_executable.data) { + current_timer_index++; continue; } any_executable.timer = timer; @@ -706,6 +709,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) valid_executable = true; break; } + current_timer_index++; } } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 831076c61c..2d837103e5 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -154,11 +154,15 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); if (entity_iter != collection.timers.end()) { wait_result.clear_timer_with_index(current_timer_index); - if (timer->call()) { - execute_timer(timer); - any_ready_executable = true; - if (spin_once) {return any_ready_executable;} + auto data = timer->call(); + if (!data) { + // someone canceled the timer between is_ready and call + continue; } + + execute_timer(std::move(timer), data); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index f7ba6da2df..ce6a103ab2 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -40,10 +40,12 @@ EventsExecutor::EventsExecutor( // The timers manager can be used either to only track timers (in this case an expired // timer will generate an executor event and then it will be executed by the executor thread) // or it can also take care of executing expired timers in its dedicated thread. - std::function timer_on_ready_cb = nullptr; + std::function &)> timer_on_ready_cb = nullptr; if (!execute_timers_separate_thread) { - timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) { - ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1}; + timer_on_ready_cb = + [this](const rclcpp::TimerBase * timer_id, const std::shared_ptr & data) { + ExecutorEvent event = {timer_id, data, -1, ExecutorEventType::TIMER_EVENT, 1}; this->events_queue_->enqueue(event); }; } @@ -88,7 +90,7 @@ EventsExecutor::EventsExecutor( } ExecutorEvent event = - {notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; + {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; this->events_queue_->enqueue(event); }); @@ -325,7 +327,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) case ExecutorEventType::TIMER_EVENT: { timers_manager_->execute_ready_timer( - static_cast(event.entity_key)); + static_cast(event.entity_key), event.data); break; } case ExecutorEventType::WAITABLE_EVENT: @@ -485,7 +487,7 @@ EventsExecutor::create_entity_callback( { std::function callback = [this, entity_key, event_type](size_t num_events) { - ExecutorEvent event = {entity_key, -1, event_type, num_events}; + ExecutorEvent event = {entity_key, nullptr, -1, event_type, num_events}; this->events_queue_->enqueue(event); }; return callback; @@ -497,7 +499,7 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) std::function callback = [this, entity_key](size_t num_events, int waitable_data) { ExecutorEvent event = - {entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events}; + {entity_key, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events}; this->events_queue_->enqueue(event); }; return callback; diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp index 39924afa56..2caa0a6b15 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -27,7 +27,7 @@ using rclcpp::experimental::TimersManager; TimersManager::TimersManager( std::shared_ptr context, - std::function on_ready_callback) + std::function &)> on_ready_callback) : on_ready_callback_(on_ready_callback), context_(context) { @@ -148,8 +148,12 @@ bool TimersManager::execute_head_timer() if (timer_ready) { // NOTE: here we always execute the timer, regardless of whether the // on_ready_callback is set or not. - head_timer->call(); - head_timer->execute_callback(); + auto data = head_timer->call(); + if (!data) { + // someone canceled the timer between is_ready and call + return false; + } + head_timer->execute_callback(data); timers_heap.heapify_root(); weak_timers_heap_.store(timers_heap); } @@ -157,7 +161,9 @@ bool TimersManager::execute_head_timer() return timer_ready; } -void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) +void TimersManager::execute_ready_timer( + const rclcpp::TimerBase * timer_id, + const std::shared_ptr & data) { TimerPtr ready_timer; { @@ -165,7 +171,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) ready_timer = weak_timers_heap_.get_timer(timer_id); } if (ready_timer) { - ready_timer->execute_callback(); + ready_timer->execute_callback(data); } } @@ -215,11 +221,16 @@ void TimersManager::execute_ready_timers_unsafe() const size_t number_ready_timers = locked_heap.get_number_ready_timers(); size_t executed_timers = 0; while (executed_timers < number_ready_timers && head_timer->is_ready()) { - head_timer->call(); - if (on_ready_callback_) { - on_ready_callback_(head_timer.get()); + auto data = head_timer->call(); + if (data) { + if (on_ready_callback_) { + on_ready_callback_(head_timer.get(), data); + } else { + head_timer->execute_callback(data); + } } else { - head_timer->execute_callback(); + // someone canceled the timer between is_ready and call + // we don't do anything, as the timer is now 'processed' } executed_timers++; diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp index de8242b55b..741e6ad384 100644 --- a/rclcpp/test/rclcpp/executors/test_events_queue.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -68,6 +68,7 @@ TEST(TestEventsQueue, SimpleQueueTest) // Lets push an event into the queue and get it back rclcpp::experimental::executors::ExecutorEvent push_event = { simple_queue.get(), + nullptr, 99, rclcpp::experimental::executors::ExecutorEventType::SUBSCRIPTION_EVENT, 1}; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index dc92dee610..ce6343fb5c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -32,8 +32,8 @@ class TestTimer : public rclcpp::TimerBase : TimerBase(node->get_clock(), std::chrono::nanoseconds(1), node->get_node_base_interface()->get_context()) {} - bool call() override {return true;} - void execute_callback() override {} + std::shared_ptr call() override {return nullptr;} + void execute_callback(const std::shared_ptr &) override {} bool is_steady() override {return false;} }; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 6d26bc54b8..55c0722ae1 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -252,6 +252,28 @@ TEST_P(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } +TEST_P(TestTimer, callback_with_timer_info) { + rclcpp::TimerInfo info; + auto timer_callback = [&info](const rclcpp::TimerInfo & timer_info) { + info = timer_info; + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } + auto start = std::chrono::steady_clock::now(); + while (info.actual_call_time.nanoseconds() == 0 && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + EXPECT_GE(info.actual_call_time, info.expected_call_time); +} + TEST_P(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) {