diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index d8d46809..381e8e07 100644 --- 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(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 last_execution_time_; }; } // namespace realtime_tools diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 105ab7a4..eb0e11b1 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -35,14 +35,12 @@ void TestAsyncFunctionHandler::initialize() std::pair 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 index e66af6bd..c4805436 100644 --- 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 handler_;