@@ -3238,7 +3238,6 @@ controller_interface::return_type ControllerManager::update(
32383238 }
32393239
32403240 PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC (hardware_interface::DEFAULT_REGISTRY_KEY);
3241- PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC (hardware_interface::CM_STATISTICS_KEY);
32423241
32433242 execution_time_.update_time =
32443243 std::chrono::duration<double , std::micro>(std::chrono::steady_clock::now () - start_time)
@@ -3340,8 +3339,7 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
33403339 " Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, "
33413340 " Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform "
33423341 " mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), "
3343- " Write "
3344- " time : %.3f us" ,
3342+ " Write time : %.3f us" ,
33453343 execution_time_.total_time , expected_cycle_time, execution_time_.read_time ,
33463344 execution_time_.update_time , execution_time_.switch_time ,
33473345 execution_time_.switch_chained_mode_time , execution_time_.switch_perform_mode_time ,
@@ -3358,6 +3356,8 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
33583356 execution_time_.update_time , execution_time_.write_time );
33593357 }
33603358 }
3359+
3360+ PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC (hardware_interface::CM_STATISTICS_KEY);
33613361}
33623362
33633363std::vector<ControllerSpec> &
0 commit comments