Added method to return the last return value of the update cycle

ros-controls · Sep 13, 2024 · 0793ba8 · 0793ba8
1 parent 495bb82
commit 0793ba8
Show file tree

Hide file tree

Showing 3 changed files with 84 additions and 1 deletion.
diff --git 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
@@ -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<bool, return_type> 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,72 @@ 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
@@ -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_;