Skip to content

Commit 0d21fd6

Browse files
saikishormergify[bot]
authored andcommitted
Fix the CM statistics async publish placement (#2865)
(cherry picked from commit f0f003f)
1 parent 812f8cd commit 0d21fd6

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
@@ -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

33623362
std::vector<ControllerSpec> &

0 commit comments

Comments
 (0)