add get last execution time retrieval time

ros-controls · Aug 7, 2024 · 305f3b8 · 305f3b8
1 parent 9420adc
commit 305f3b8
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 6 deletions.
diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp
@@ -211,6 +211,12 @@ class AsyncFunctionHandler
     }
   }
 
+  /// Get the last execution time of the async callback method
+  /**
+   * @return The last execution time of the async callback method in seconds
+   */
+  double get_last_execution_time() const { return last_execution_time_; }
+
   /// Initializes and starts the callback thread
   /**
    * If the worker thread is not running, it will start the async callback thread.
@@ -225,6 +231,7 @@ class AsyncFunctionHandler
     if (!thread_.joinable()) {
       stop_async_callback_ = false;
       trigger_in_progress_ = false;
+      last_execution_time_ = 0.0;
       async_callback_return_ = T();
       thread_ = std::thread([this]() -> void {
         if (!realtime_tools::configure_sched_fifo(thread_priority_)) {
@@ -243,8 +250,11 @@ class AsyncFunctionHandler
             async_callback_condition_.wait(
               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_);
+              const auto end_time = std::chrono::steady_clock::now();
+              last_execution_time_ = std::chrono::duration<double>(end_time - start_time).count();
             }
             trigger_in_progress_ = false;
           }
@@ -270,6 +280,7 @@ class AsyncFunctionHandler
   std::condition_variable async_callback_condition_;
   std::condition_variable cycle_end_condition_;
   std::mutex async_mtx_;
+  std::atomic<double> last_execution_time_;
 };
 }  // namespace realtime_tools
 

diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp
@@ -35,14 +35,12 @@ void TestAsyncFunctionHandler::initialize()
 
 std::pair<bool, return_type> TestAsyncFunctionHandler::trigger()
 {
-  return handler_.trigger_async_callback(last_callback_time_, last_callback_period_);
+  return handler_.trigger_async_callback(rclcpp::Time(0, 0), rclcpp::Duration(0, 0));
 }
 
 return_type TestAsyncFunctionHandler::update(
-  const rclcpp::Time & time, const rclcpp::Duration & period)
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
-  last_callback_time_ = time;
-  last_callback_period_ = period;
   // to simulate some work being done
   std::this_thread::sleep_for(std::chrono::microseconds(10));
   counter_++;
@@ -126,6 +124,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering)
   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);
 

diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp
@@ -52,8 +52,6 @@ class TestAsyncFunctionHandler
 
 private:
   rclcpp_lifecycle::State state_;
-  rclcpp::Time last_callback_time_;
-  rclcpp::Duration last_callback_period_{0, 0};
   int counter_;
   return_type return_state_;
   realtime_tools::AsyncFunctionHandler<return_type> handler_;