diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 381e8e07..506969aa 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -110,6 +110,10 @@ class AsyncFunctionHandler * If the async callback method is still running, it will return the last return value from the * last trigger cycle. * + * \note If an exception is caught in the async callback thread, it will be rethrown in the current + * thread, so in order to have the trigger_async_callback method working again, the exception should + * be caught and the `reset_variables` method should be invoked. + * * \note In the case of controllers, The controller manager is responsible * for triggering and maintaining the controller's update rate, as it should be only acting as a * scheduler. Same applies to the resource manager when handling the hardware components. @@ -120,6 +124,12 @@ class AsyncFunctionHandler if (!is_initialized()) { throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); } + if (async_exception_ptr_) { + RCLCPP_ERROR( + rclcpp::get_logger("AsyncFunctionHandler"), + "AsyncFunctionHandler: Exception caught in the async callback thread!"); + std::rethrow_exception(async_exception_ptr_); + } if (!is_running()) { throw std::runtime_error( "AsyncFunctionHandler: need to start the async callback thread first before triggering!"); @@ -140,6 +150,30 @@ class AsyncFunctionHandler return std::make_pair(trigger_status, return_value); } + /// Get the last return value of the async callback method + /** + * @return The last return value of the async callback method + */ + T get_last_return_value() const { return async_callback_return_; } + + /// Resets the internal variables of the AsyncFunctionHandler + /** + * A method to reset the internal variables of the AsyncFunctionHandler. + * It will reset the async callback return value, exception pointer, and the trigger status. + * + * \note This method should be invoked after catching an exception in the async callback thread, + * to be able to trigger the async callback method again. + */ + void reset_variables() + { + std::unique_lock lock(async_mtx_); + stop_async_callback_ = false; + trigger_in_progress_ = false; + last_execution_time_ = 0.0; + async_callback_return_ = T(); + async_exception_ptr_ = nullptr; + } + /// Waits until the current async callback method trigger cycle is finished /** * If the async method is running, it will wait for the current async method call to finish. @@ -229,10 +263,7 @@ class AsyncFunctionHandler throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); } if (!thread_.joinable()) { - stop_async_callback_ = false; - trigger_in_progress_ = false; - last_execution_time_ = 0.0; - async_callback_return_ = T(); + reset_variables(); thread_ = std::thread([this]() -> void { if (!realtime_tools::configure_sched_fifo(thread_priority_)) { RCLCPP_WARN( @@ -251,8 +282,12 @@ class AsyncFunctionHandler lock, [this] { return trigger_in_progress_ || stop_async_callback_; }); if (!stop_async_callback_) { const auto start_time = std::chrono::steady_clock::now(); - async_callback_return_ = - async_function_(current_callback_time_, current_callback_period_); + try { + async_callback_return_ = + async_function_(current_callback_time_, current_callback_period_); + } catch (...) { + async_exception_ptr_ = std::current_exception(); + } const auto end_time = std::chrono::steady_clock::now(); last_execution_time_ = std::chrono::duration(end_time - start_time).count(); } @@ -281,6 +316,7 @@ class AsyncFunctionHandler std::condition_variable cycle_end_condition_; std::mutex async_mtx_; std::atomic last_execution_time_; + std::exception_ptr async_exception_ptr_; }; } // namespace realtime_tools diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index eb0e11b1..451a3796 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_async_function_handler.hpp" +#include + #include "gmock/gmock.h" #include "rclcpp/rclcpp.hpp" +#include "test_async_function_handler.hpp" namespace realtime_tools { @@ -23,6 +25,7 @@ TestAsyncFunctionHandler::TestAsyncFunctionHandler() counter_(0), return_state_(return_type::OK) { + reset_counter(0); } void TestAsyncFunctionHandler::initialize() @@ -30,7 +33,11 @@ void TestAsyncFunctionHandler::initialize() handler_.init( std::bind( &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2), - [this]() { return state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; }); + [this]() { + return ( + state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + handler_.get_last_return_value() != realtime_tools::return_type::DEACTIVATE); + }); } std::pair TestAsyncFunctionHandler::trigger() @@ -41,6 +48,9 @@ std::pair TestAsyncFunctionHandler::trigger() return_type TestAsyncFunctionHandler::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + if (counter_ == std::numeric_limits::max()) { + throw std::overflow_error("Counter reached maximum value"); + } // to simulate some work being done std::this_thread::sleep_for(std::chrono::microseconds(10)); counter_++; @@ -60,6 +70,13 @@ void TestAsyncFunctionHandler::deactivate() state_ = rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state_.label()); } +void TestAsyncFunctionHandler::reset_counter(int counter) { counter_ = counter; } + +void TestAsyncFunctionHandler::set_return_state(return_type return_state) +{ + return_state_ = return_state; +} + } // namespace realtime_tools class AsyncFunctionHandlerTest : public ::testing::Test { @@ -255,3 +272,138 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_TRUE(async_class.get_handler().is_stopped()); } + +TEST_F(AsyncFunctionHandlerTest, check_triggering_with_different_return_state_and_predicate) +{ + realtime_tools::TestAsyncFunctionHandler async_class; + async_class.initialize(); + async_class.get_handler().start_thread(); + + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + auto trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + 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_TRUE(async_class.get_handler().get_thread().joinable()); + ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + async_class.get_handler().get_last_execution_time(); + ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::OK); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ(async_class.get_counter(), 1); + + // Trigger one more cycle to return ERROR at the end of cycle, + // so return from this cycle should be last cycle's return + async_class.set_return_state(realtime_tools::return_type::ERROR); + trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + 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_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::OK); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::ERROR); + ASSERT_LE(async_class.get_counter(), 2); + + // Trigger one more cycle to return DEACTIVATE at the end of cycle, + async_class.set_return_state(realtime_tools::return_type::DEACTIVATE); + trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::ERROR, trigger_status.second); + 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_FALSE(async_class.get_handler().is_stopped()); + ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::ERROR); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ( + async_class.get_handler().get_last_return_value(), realtime_tools::return_type::DEACTIVATE); + ASSERT_LE(async_class.get_counter(), 3); + + // Now the next trigger shouldn't happen as the predicate is set to DEACTIVATE + trigger_status = async_class.trigger(); + ASSERT_FALSE(trigger_status.first) << "The trigger should fail as the predicate is DEACTIVATE"; + ASSERT_EQ(realtime_tools::return_type::DEACTIVATE, trigger_status.second); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); + + async_class.get_handler().stop_thread(); + // now the async update should be preempted + ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); +} + +TEST_F(AsyncFunctionHandlerTest, check_exception_handling) +{ + realtime_tools::TestAsyncFunctionHandler async_class; + async_class.initialize(); + async_class.get_handler().start_thread(); + + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + auto trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + 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_TRUE(async_class.get_handler().get_thread().joinable()); + ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + async_class.get_handler().get_last_execution_time(); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ(async_class.get_counter(), 1); + + // Trigger one more cycle + async_class.reset_counter(std::numeric_limits::max()); + trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + ASSERT_LE(async_class.get_counter(), std::numeric_limits::max()); + + std::this_thread::sleep_for(std::chrono::microseconds(10)); + // Trigger one more cycle to see exception handling + ASSERT_THROW(async_class.trigger(), std::overflow_error); + + // now the async update should be preempted as there was an exception + std::this_thread::sleep_for(std::chrono::microseconds(10)); + ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + + // Should rethrow the exception unless the reset_variables is called + for (int i = 0; i < 50; i++) { + ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_THROW(async_class.trigger(), std::overflow_error); + } + async_class.get_handler().reset_variables(); + + async_class.reset_counter(0); + async_class.get_handler().start_thread(); + trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + 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_TRUE(async_class.get_handler().get_thread().joinable()); + ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); + async_class.get_handler().get_last_execution_time(); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); + ASSERT_EQ(async_class.get_counter(), 1); + async_class.get_handler().stop_thread(); + ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); +} diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp index c4805436..c36d6db1 100644 --- a/test/test_async_function_handler.hpp +++ b/test/test_async_function_handler.hpp @@ -50,6 +50,10 @@ class TestAsyncFunctionHandler void deactivate(); + void reset_counter(int counter = 0); + + void set_return_state(return_type return_state); + private: rclcpp_lifecycle::State state_; int counter_;