From 1a57fdf4ef34d4dd65ce2bbde69770ac986d5504 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:46:05 +0100 Subject: [PATCH] use current trigger time from AsyncFunctionHandler to calculate the right periodicity --- .../controller_interface/controller_interface_base.hpp | 1 + controller_interface/src/controller_interface_base.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2299d59107..bff7e204ad 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -77,6 +77,7 @@ struct ControllerUpdateStatus bool ok = true; return_type result = return_type::OK; std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; }; /** diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index e02ff23b82..492a7ee1bc 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -166,6 +166,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( trigger_stats_.total_triggers++; if (is_async()) { + const rclcpp::Time & last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); if (!result.first) { @@ -182,6 +183,10 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( { status.execution_time = execution_time; } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } } else { @@ -190,6 +195,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( status.result = update(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; }