From f4e3f5a8ebf40aee63f896208d542dbc55e787c2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 29 Jan 2025 19:55:08 +0100 Subject: [PATCH] Fix the initial wrong periodicity reported by controller_manager (#2018) --- controller_manager/src/ros2_control_node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 75a98d19dd..176c346b3f 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -93,6 +93,10 @@ int main(int argc, char ** argv) } } + // wait for the clock to be available + cm->get_clock()->wait_until_started(); + cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( @@ -122,7 +126,7 @@ int main(int argc, char ** argv) auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds()); std::chrono::time_point - next_iteration_time{cm_now}; + next_iteration_time{cm_now - period}; // for calculating the measured period of the loop rclcpp::Time previous_time = cm->now() - period;