use current trigger time from AsyncFunctionHandler to calculate the r…

…ight periodicity
ros-controls · Nov 17, 2024 · 1a57fdf · 1a57fdf
1 parent e873706
commit 1a57fdf
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git 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<std::chrono::nanoseconds> execution_time = std::nullopt;
+  std::optional<rclcpp::Duration> period = std::nullopt;
 };
 
 /**

diff --git 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::nanoseconds>(
       std::chrono::steady_clock::now() - start_time);
+    status.period = period;
   }
   return status;
 }