diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 108ddbd..506969a 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -150,6 +150,12 @@ 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. diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index d7f9d97..5fd11a4 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -33,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() @@ -68,6 +72,11 @@ void TestAsyncFunctionHandler::deactivate() } 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 { @@ -264,6 +273,73 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) 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; diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp index 9364d57..c36d6db 100644 --- a/test/test_async_function_handler.hpp +++ b/test/test_async_function_handler.hpp @@ -52,6 +52,8 @@ class TestAsyncFunctionHandler void reset_counter(int counter = 0); + void set_return_state(return_type return_state); + private: rclcpp_lifecycle::State state_; int counter_;