@@ -3237,7 +3237,6 @@ controller_interface::return_type ControllerManager::update(
32373237 }
32383238
32393239 PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC (hardware_interface::DEFAULT_REGISTRY_KEY);
3240- PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC (hardware_interface::CM_STATISTICS_KEY);
32413240
32423241 execution_time_.update_time =
32433242 std::chrono::duration<double , std::micro>(std::chrono::steady_clock::now () - start_time)
@@ -3339,8 +3338,7 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
33393338 " Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, "
33403339 " Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform "
33413340 " mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), "
3342- " Write "
3343- " time : %.3f us" ,
3341+ " Write time : %.3f us" ,
33443342 execution_time_.total_time , expected_cycle_time, execution_time_.read_time ,
33453343 execution_time_.update_time , execution_time_.switch_time ,
33463344 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
33573355 execution_time_.update_time , execution_time_.write_time );
33583356 }
33593357 }
3358+
3359+ PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC (hardware_interface::CM_STATISTICS_KEY);
33603360}
33613361
33623362std::vector<ControllerSpec> &
0 commit comments