diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 46bd83f24c1..732addeccd1 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -99,7 +99,15 @@ class AsyncFunctionHandler std::pair trigger_async_update( const rclcpp::Time & time, const rclcpp::Duration & period) { - initialize_async_update_thread(); + if (!is_initialized()) + { + throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); + } + if (!is_running()) + { + throw std::runtime_error( + "AsyncFunctionHandler: need to start the async update thread first before triggering!"); + } std::unique_lock lock(async_mtx_, std::try_to_lock); bool trigger_status = false; if (lock.owns_lock() && !trigger_in_progress_) @@ -185,14 +193,13 @@ class AsyncFunctionHandler } } -private: /// Initialize the async update thread /** * If the async update thread is not running, it will start the async update thread. * If the async update thread is already configured and running, does nothing and return * immediately. */ - void initialize_async_update_thread() + void start_async_update_thread() { if (!is_initialized()) { @@ -209,7 +216,9 @@ class AsyncFunctionHandler // \note There might be an concurrency issue with the get_state() call here. This mightn't // be critical here as the state of the controller is not expected to change during the // update cycle - while (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + while ((get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || + get_state_function_().id() == + lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) && !async_update_stop_) { { @@ -227,10 +236,13 @@ class AsyncFunctionHandler } cycle_end_condition_.notify_one(); } + trigger_in_progress_ = false; + cycle_end_condition_.notify_one(); }); } } +private: rclcpp::Time current_update_time_; rclcpp::Duration current_update_period_{0, 0}; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index bd4398bf633..ebe0fbb54f6 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -58,8 +58,15 @@ return_type ControllerInterfaceBase::init( node_->register_on_cleanup( std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); - node_->register_on_activate( - std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); + auto activate_callback = [this](const rclcpp_lifecycle::State & state) + { + if (is_async() && async_handler_) + { + async_handler_->start_async_update_thread(); + } + return on_activate(state); + }; + node_->register_on_activate(activate_callback); node_->register_on_deactivate( std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1)); diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index f6ac2d77ca4..70cb96370d1 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -87,13 +87,12 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_FALSE(async_class.get_handler().is_stopped()); // Once initialized, it should not be possible to initialize again + async_class.get_handler().start_async_update_thread(); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); ASSERT_THROW(async_class.initialize(), std::runtime_error); - // The preempt_async_update is already called with the destructor - // async_class.get_handler().preempt_async_update(); } TEST_F(AsyncFunctionHandlerTest, check_triggering) @@ -110,6 +109,9 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); + // It shouldn't be possible to trigger without starting the thread + ASSERT_THROW(async_class.trigger(), std::runtime_error); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); @@ -143,6 +145,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); for (int i = 1; i < 1e4; i++) @@ -172,20 +175,19 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_TRUE(async_class.get_handler().is_stopped()); + async_class.get_handler().start_async_update_thread(); // The thread will start and end immediately when invoked in inactive state - for (size_t i = 0; i < 3; i++) - { - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - async_class.get_handler().join_async_update_thread(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - } + async_class.get_handler().join_async_update_thread(); + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_THROW(async_class.trigger(), std::runtime_error); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); // Now activate it and launch again async_class.activate(); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); for (int i = 1; i < 100; i++) { @@ -211,6 +213,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) // Now let's test the case of activating it and then deactivating it when the thread is waiting // for a trigger to start new update cycle execution async_class.activate(); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger());