From 5971f919768eb0bbc690b5d6f64307f908a5a47c Mon Sep 17 00:00:00 2001 From: Eloy Briceno <51831786+Voldivh@users.noreply.github.com> Date: Wed, 21 Jun 2023 09:47:14 -0500 Subject: [PATCH] Modifies timers API to select autostart state (#2005) * Modifies timers API to select autostart state * Removes unnecessary variables * Adds autostart documentation and expands some timer test Signed-off-by: Voldivh --- rclcpp/include/rclcpp/create_timer.hpp | 23 +++++++++++------- rclcpp/include/rclcpp/node.hpp | 4 +++- rclcpp/include/rclcpp/node_impl.hpp | 6 +++-- rclcpp/include/rclcpp/timer.hpp | 18 ++++++++++---- rclcpp/src/rclcpp/timer.cpp | 9 +++---- rclcpp/test/rclcpp/test_create_timer.cpp | 26 ++++++++++++++++++++ rclcpp/test/rclcpp/test_timer.cpp | 30 ++++++++++++++++++++++++ 7 files changed, 96 insertions(+), 20 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index d371466f0d..64d5b8e322 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -90,7 +90,8 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( clock, @@ -98,7 +99,8 @@ create_timer( std::forward(callback), group, node_base.get(), - node_timers.get()); + node_timers.get(), + autostart); } /// Create a timer with a given clock @@ -109,7 +111,8 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( clock, @@ -117,7 +120,8 @@ create_timer( std::forward(callback), group, rclcpp::node_interfaces::get_node_base_interface(node).get(), - rclcpp::node_interfaces::get_node_timers_interface(node).get()); + rclcpp::node_interfaces::get_node_timers_interface(node).get(), + autostart); } /// Convenience method to create a general timer with node resources. @@ -132,6 +136,7 @@ create_timer( * \param group callback group * \param node_base node base interface * \param node_timers node timer interface + * \param autostart defines if the timer should start it's countdown on initialization or not. * \return shared pointer to a generic timer * \throws std::invalid_argument if either clock, node_base or node_timers * are nullptr, or period is negative or too large @@ -144,7 +149,8 @@ create_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) { if (clock == nullptr) { throw std::invalid_argument{"clock cannot be null"}; @@ -160,7 +166,7 @@ create_timer( // Add a new generic timer. auto timer = rclcpp::GenericTimer::make_shared( - std::move(clock), period_ns, std::move(callback), node_base->get_context()); + std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } @@ -187,7 +193,8 @@ create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) { if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; @@ -201,7 +208,7 @@ create_wall_timer( // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( - period_ns, std::move(callback), node_base->get_context()); + period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 308de8fafc..d512172d2b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -233,13 +233,15 @@ class Node : public std::enable_shared_from_this * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. + * \param[in] autostart The state of the clock on initialization. */ template typename rclcpp::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true); /// Create a timer that uses the node clock to drive the callback. /** diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..5f1fb7b3df 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -110,14 +110,16 @@ typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + bool autostart) { return rclcpp::create_wall_timer( period, std::move(callback), group, this->node_base_.get(), - this->node_timers_.get()); + this->node_timers_.get(), + autostart); } template diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index cc40736ee9..6060d8bd78 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -53,12 +53,17 @@ class TimerBase * \param clock A clock to use for time and sleeping * \param period The interval at which the timer fires * \param context node context + * \param autostart timer state on initialization + * + * In order to activate a timer that is not started on initialization, + * user should call the reset() method. */ RCLCPP_PUBLIC explicit TimerBase( Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context); + rclcpp::Context::SharedPtr context, + bool autostart = true); /// TimerBase destructor RCLCPP_PUBLIC @@ -216,12 +221,13 @@ class GenericTimer : public TimerBase * \param[in] period The interval at which the timer fires. * \param[in] callback User-specified callback function. * \param[in] context custom context to be used. + * \param autostart timer state on initialization */ explicit GenericTimer( Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, - rclcpp::Context::SharedPtr context + rclcpp::Context::SharedPtr context, bool autostart = true ) - : TimerBase(clock, period, context), callback_(std::forward(callback)) + : TimerBase(clock, period, context, autostart), callback_(std::forward(callback)) { TRACETOOLS_TRACEPOINT( rclcpp_timer_callback_added, @@ -330,13 +336,15 @@ class WallTimer : public GenericTimer * \param period The interval at which the timer fires * \param callback The callback function to execute every interval * \param context node context + * \param autostart timer state on initialization */ WallTimer( std::chrono::nanoseconds period, FunctorT && callback, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + bool autostart = true) : GenericTimer( - std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context) + std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context, autostart) {} protected: diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 59ddd5e8dd..0dceb6b8d7 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -32,7 +32,8 @@ using rclcpp::TimerBase; TimerBase::TimerBase( rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + bool autostart) : clock_(clock), timer_handle_(nullptr) { if (nullptr == context) { @@ -64,9 +65,9 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - rcl_ret_t ret = rcl_timer_init( - timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, - rcl_get_default_allocator()); + rcl_ret_t ret = rcl_timer_init2( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), + nullptr, rcl_get_default_allocator(), autostart); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); } diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f253af4838..c4dc1f7e61 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -193,3 +193,29 @@ TEST(TestCreateTimer, timer_function_pointer) rclcpp::shutdown(); } + +TEST(TestCreateTimer, timer_without_autostart) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_create_timer_node"); + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + []() {}, + nullptr, + false); + + EXPECT_TRUE(timer->is_canceled()); + EXPECT_EQ(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + + timer->reset(); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer->is_canceled()); + + timer->cancel(); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 59a1218519..33ded455c3 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -73,6 +73,20 @@ class TestTimer : public ::testing::TestWithParam EXPECT_FALSE(timer->is_steady()); break; } + timer_without_autostart = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + }, nullptr, false); + EXPECT_TRUE(timer_without_autostart->is_steady()); + executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -93,6 +107,7 @@ class TestTimer : public ::testing::TestWithParam std::atomic cancel_timer; rclcpp::Node::SharedPtr test_node; std::shared_ptr timer; + std::shared_ptr timer_without_autostart; std::shared_ptr executor; }; @@ -334,3 +349,18 @@ INSTANTIATE_TEST_SUITE_P( return std::string("unknown"); } ); + +/// Simple test of a timer without autostart +TEST_P(TestTimer, test_timer_without_autostart) +{ + EXPECT_TRUE(timer_without_autostart->is_canceled()); + EXPECT_EQ( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + // Reset to change start timer + timer_without_autostart->reset(); + EXPECT_LE( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer_without_autostart->is_canceled()); +}