keep the thread alive for the realtimeness and delegate to the user

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.
@@ -121,10 +125,9 @@ class AsyncFunctionHandler
       throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!");
     }
     if (async_exception_ptr_) {
-      RCLCPP_FATAL(
+      RCLCPP_ERROR(
         rclcpp::get_logger("AsyncFunctionHandler"),
         "AsyncFunctionHandler: Exception caught in the async callback thread!");
-      join_async_callback_thread();
       std::rethrow_exception(async_exception_ptr_);
     }
     if (!is_running()) {
@@ -147,6 +150,24 @@ class AsyncFunctionHandler
     return std::make_pair(trigger_status, return_value);
   }
 
+  /// 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.
@@ -232,15 +253,11 @@ class AsyncFunctionHandler
    */
   void start_thread()
   {
-    async_exception_ptr_ = nullptr;
     if (!is_initialized()) {
       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(
@@ -264,9 +281,6 @@ class AsyncFunctionHandler
                   async_function_(current_callback_time_, current_callback_period_);
               } catch (...) {
                 async_exception_ptr_ = std::current_exception();
-                stop_async_callback_ = true;
-                trigger_in_progress_ = false;
-                break;
               }
               const auto end_time = std::chrono::steady_clock::now();
               last_execution_time_ = std::chrono::duration<double>(end_time - start_time).count();

test/test_async_function_handler.cpp

@@ -300,12 +300,16 @@ TEST_F(AsyncFunctionHandlerTest, check_exception_handling)
 
   // now the async update should be preempted as there was an exception
   std::this_thread::sleep_for(std::chrono::microseconds(10));
-  ASSERT_FALSE(async_class.get_handler().is_running());
-  ASSERT_TRUE(async_class.get_handler().is_stopped());
+  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 thread is restarted
-  ASSERT_THROW(async_class.trigger(), std::overflow_error);
+  // 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();