[AsyncFunctionHandler] Add exception handling

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<std::mutex> 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<double>(end_time - start_time).count();
             }
@@ -281,6 +316,7 @@ class AsyncFunctionHandler
   std::condition_variable cycle_end_condition_;
   std::mutex async_mtx_;
   std::atomic<double> last_execution_time_;
+  std::exception_ptr async_exception_ptr_;
 };
 }  // namespace realtime_tools
 

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 <limits>
+
 #include "gmock/gmock.h"
 #include "rclcpp/rclcpp.hpp"
+#include "test_async_function_handler.hpp"
 
 namespace realtime_tools
 {
@@ -23,14 +25,19 @@ TestAsyncFunctionHandler::TestAsyncFunctionHandler()
   counter_(0),
   return_state_(return_type::OK)
 {
+  reset_counter(0);
 }
 
 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()
@@ -41,6 +48,9 @@ std::pair<bool, return_type> TestAsyncFunctionHandler::trigger()
 return_type TestAsyncFunctionHandler::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
+  if (counter_ == std::numeric_limits<int>::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<int>::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<int>::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();
+}

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_;