From 0d21fd65ff20597ac87727ce31b25689bea03154 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 29 Nov 2025 13:45:47 +0100 Subject: [PATCH] Fix the CM statistics async publish placement (#2865) (cherry picked from commit f0f003f01924b13fec126348b457aeaebc3cd6d3) --- controller_manager/src/controller_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f3c5bbf1a..1cf274bc6f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3237,7 +3237,6 @@ controller_interface::return_type ControllerManager::update( } PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::DEFAULT_REGISTRY_KEY); - PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::CM_STATISTICS_KEY); execution_time_.update_time = std::chrono::duration(std::chrono::steady_clock::now() - start_time) @@ -3339,8 +3338,7 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration "Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, " "Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform " "mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), " - "Write " - "time : %.3f us", + "Write time : %.3f us", execution_time_.total_time, expected_cycle_time, execution_time_.read_time, execution_time_.update_time, execution_time_.switch_time, execution_time_.switch_chained_mode_time, execution_time_.switch_perform_mode_time, @@ -3357,6 +3355,8 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration execution_time_.update_time, execution_time_.write_time); } } + + PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::CM_STATISTICS_KEY); } std::vector &