Skip to content

Commit 70615e9

Browse files
Fix the CM statistics async publish placement (#2865) (#2869)
(cherry picked from commit f0f003f) Co-authored-by: Sai Kishor Kothakota <[email protected]>
1 parent b8e682a commit 70615e9

File tree

1 file changed

+3
-3
lines changed

1 file changed

+3
-3
lines changed

controller_manager/src/controller_manager.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -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

33633363
std::vector<ControllerSpec> &

0 commit comments

Comments
 (0)