From 7e60b67655ca587bfe8ef82d7961c85a88a977ac Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Feb 2025 09:51:26 +0100 Subject: [PATCH 01/12] change logic to fix the bad periodict on startup --- controller_manager/src/ros2_control_node.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 176c346b3f..f14e5105ab 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -124,12 +124,14 @@ int main(int argc, char ** argv) // for calculating sleep time 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 - period}; // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now() - period; + rclcpp::Time previous_time = cm->now(); + std::this_thread::sleep_for(period); + + auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds()); + std::chrono::time_point + next_iteration_time{cm_now}; while (rclcpp::ok()) { From aa2b271a698dc13a3f1dcef572e13ea75cdd3060 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Feb 2025 17:20:08 +0100 Subject: [PATCH 02/12] Handle overruns in the controller manager --- controller_manager/doc/userdoc.rst | 4 +++ controller_manager/src/ros2_control_node.cpp | 29 +++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 7e8ffb52fa..e139b68d7e 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -361,6 +361,10 @@ cpu_affinity (optional; int (or) int_array;) thread_priority (optional; int; default: 50) Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99. +overrun_wait_period (optional; double; default: 10% of the control period) + Sets the time to wait for the control loop to catch up after an overrun. + Overrun is triggered when the control loop takes longer than the control period to execute. + use_sim_time (optional; bool; default: false) Enables the use of simulation time in the ``controller_manager`` node. diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index f14e5105ab..a881b412ac 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -93,6 +93,21 @@ int main(int argc, char ** argv) } } + const double overrun_wait_time = + cm->get_parameter_or("overrun_wait_period", (0.1 / cm->get_update_rate())); + if (overrun_wait_time < 0.0) + { + RCLCPP_ERROR( + cm->get_logger(), + "The parsed overrun_wait_period should be a positive value. The parsed value is %f.", + overrun_wait_time); + return 1; + } + RCLCPP_WARN_EXPRESSION( + cm->get_logger(), overrun_wait_time > (1.0 / cm->get_update_rate()), + "The parsed overrun_wait_period is larger than the period of the controller manager."); + const std::chrono::nanoseconds overrun_wait_period(static_cast(overrun_wait_time * 1e9)); + // 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())); @@ -104,7 +119,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time]() + [cm, thread_priority, use_sim_time, overrun_wait_period]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -153,6 +168,18 @@ int main(int argc, char ** argv) } else { + const std::chrono::time_point + time_now{std::chrono::nanoseconds(cm->now().nanoseconds())}; + if (next_iteration_time < time_now) + { + RCLCPP_WARN( + cm->get_logger(), + "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " + "took %f ms.", + cm->get_update_rate(), + static_cast((time_now - next_iteration_time).count()) / 1e6); + next_iteration_time = time_now + overrun_wait_period; + } std::this_thread::sleep_until(next_iteration_time); } } From 7c9ad2d942d953c9d91917803a0d3785b9dd0ffa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Feb 2025 16:41:15 +0100 Subject: [PATCH 03/12] fix the overrun wait time functionality --- controller_manager/doc/userdoc.rst | 4 --- controller_manager/src/ros2_control_node.cpp | 34 +++++++------------- 2 files changed, 11 insertions(+), 27 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index e139b68d7e..7e8ffb52fa 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -361,10 +361,6 @@ cpu_affinity (optional; int (or) int_array;) thread_priority (optional; int; default: 50) Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99. -overrun_wait_period (optional; double; default: 10% of the control period) - Sets the time to wait for the control loop to catch up after an overrun. - Overrun is triggered when the control loop takes longer than the control period to execute. - use_sim_time (optional; bool; default: false) Enables the use of simulation time in the ``controller_manager`` node. diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index a881b412ac..e5c8e07918 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -93,21 +93,6 @@ int main(int argc, char ** argv) } } - const double overrun_wait_time = - cm->get_parameter_or("overrun_wait_period", (0.1 / cm->get_update_rate())); - if (overrun_wait_time < 0.0) - { - RCLCPP_ERROR( - cm->get_logger(), - "The parsed overrun_wait_period should be a positive value. The parsed value is %f.", - overrun_wait_time); - return 1; - } - RCLCPP_WARN_EXPRESSION( - cm->get_logger(), overrun_wait_time > (1.0 / cm->get_update_rate()), - "The parsed overrun_wait_period is larger than the period of the controller manager."); - const std::chrono::nanoseconds overrun_wait_period(static_cast(overrun_wait_time * 1e9)); - // 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())); @@ -119,7 +104,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time, overrun_wait_period]() + [cm, thread_priority, use_sim_time]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -145,8 +130,7 @@ int main(int argc, char ** argv) std::this_thread::sleep_for(period); auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds()); - std::chrono::time_point - next_iteration_time{cm_now}; + std::chrono::steady_clock::time_point next_iteration_time{cm_now}; while (rclcpp::ok()) { @@ -168,17 +152,21 @@ int main(int argc, char ** argv) } else { - const std::chrono::time_point - time_now{std::chrono::nanoseconds(cm->now().nanoseconds())}; + const std::chrono::steady_clock::time_point time_now{ + std::chrono::nanoseconds(cm->now().nanoseconds())}; if (next_iteration_time < time_now) { + const int overrun_count = static_cast(std::ceil( + static_cast(time_now.time_since_epoch().count()) / + static_cast(next_iteration_time.time_since_epoch().count()))); RCLCPP_WARN( cm->get_logger(), "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " - "took %f ms.", + "took %f ms (missed cycles : %d).", cm->get_update_rate(), - static_cast((time_now - next_iteration_time).count()) / 1e6); - next_iteration_time = time_now + overrun_wait_period; + static_cast((time_now - next_iteration_time).count() + period.count()) / 1e6, + overrun_count + 1); + next_iteration_time += (overrun_count * period); } std::this_thread::sleep_until(next_iteration_time); } From 748a6cb06cad1e4f25b7b451cad74f2d201c4531 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Feb 2025 16:53:22 +0100 Subject: [PATCH 04/12] improve logic --- controller_manager/src/ros2_control_node.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index e5c8e07918..2c49243954 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -156,16 +156,14 @@ int main(int argc, char ** argv) std::chrono::nanoseconds(cm->now().nanoseconds())}; if (next_iteration_time < time_now) { - const int overrun_count = static_cast(std::ceil( - static_cast(time_now.time_since_epoch().count()) / - static_cast(next_iteration_time.time_since_epoch().count()))); + const double time_diff = static_cast((time_now - next_iteration_time).count()); + const double cm_period = 1.0 / static_cast(cm->get_update_rate()); + const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); RCLCPP_WARN( cm->get_logger(), "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " "took %f ms (missed cycles : %d).", - cm->get_update_rate(), - static_cast((time_now - next_iteration_time).count() + period.count()) / 1e6, - overrun_count + 1); + cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); next_iteration_time += (overrun_count * period); } std::this_thread::sleep_until(next_iteration_time); From 8df6baf67babbf69e890af215e4a6348c284d92e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 6 Feb 2025 10:12:17 +0100 Subject: [PATCH 05/12] Change to system_clock chrono type --- controller_manager/src/ros2_control_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 2c49243954..3a126df69a 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -130,7 +130,7 @@ int main(int argc, char ** argv) std::this_thread::sleep_for(period); auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds()); - std::chrono::steady_clock::time_point next_iteration_time{cm_now}; + std::chrono::system_clock::time_point next_iteration_time{cm_now}; while (rclcpp::ok()) { @@ -152,7 +152,7 @@ int main(int argc, char ** argv) } else { - const std::chrono::steady_clock::time_point time_now{ + const std::chrono::system_clock::time_point time_now{ std::chrono::nanoseconds(cm->now().nanoseconds())}; if (next_iteration_time < time_now) { From baa665516848c6eb14bbb591ce4db030f7684b91 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 6 Feb 2025 14:23:22 +0100 Subject: [PATCH 06/12] optimize the check --- controller_manager/src/ros2_control_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 3a126df69a..7394fb086c 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -152,11 +152,11 @@ int main(int argc, char ** argv) } else { - const std::chrono::system_clock::time_point time_now{ - std::chrono::nanoseconds(cm->now().nanoseconds())}; - if (next_iteration_time < time_now) + const auto time_now = cm->now().nanoseconds(); + if (next_iteration_time.time_since_epoch().count() < time_now) { - const double time_diff = static_cast((time_now - next_iteration_time).count()); + const double time_diff = + static_cast((time_now - next_iteration_time.time_since_epoch().count())); const double cm_period = 1.0 / static_cast(cm->get_update_rate()); const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); RCLCPP_WARN( From 0a0d707c4e2a6f7b5d5f908c91eb6fbeece96be5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 6 Feb 2025 17:52:48 +0100 Subject: [PATCH 07/12] change to milliseconds the variables --- controller_manager/src/ros2_control_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 7394fb086c..f43136e6b0 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -156,8 +156,8 @@ int main(int argc, char ** argv) if (next_iteration_time.time_since_epoch().count() < time_now) { const double time_diff = - static_cast((time_now - next_iteration_time.time_since_epoch().count())); - const double cm_period = 1.0 / static_cast(cm->get_update_rate()); + static_cast((time_now - next_iteration_time.time_since_epoch().count())) / 1.e6; + const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); RCLCPP_WARN( cm->get_logger(), From 5b4cb988c6365ddcc1e1569ae626a7ab81a0b2df Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 6 Feb 2025 18:56:58 +0100 Subject: [PATCH 08/12] Use steady clock instead of ROS clock for the controller manager --- controller_manager/src/ros2_control_node.cpp | 21 +++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index f43136e6b0..2550bfabf6 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -53,13 +53,18 @@ int main(int argc, char ** argv) node_arguments.push_back(argv[i]); } cm_node_options.arguments(node_arguments); + const bool has_realtime = realtime_tools::has_realtime_kernel(); + const auto cm_clock_type = has_realtime ? RCL_STEADY_TIME : RCL_ROS_TIME; + cm_node_options.clock_type(cm_clock_type); auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + RCLCPP_INFO( + cm->get_logger(), "Starting controller manager using %s clock", + has_realtime ? "STEADY" : "ROS"); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); - const bool has_realtime = realtime_tools::has_realtime_kernel(); const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); if (lock_memory) { @@ -129,8 +134,7 @@ int main(int argc, char ** argv) rclcpp::Time previous_time = cm->now(); std::this_thread::sleep_for(period); - auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds()); - std::chrono::system_clock::time_point next_iteration_time{cm_now}; + std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; while (rclcpp::ok()) { @@ -145,18 +149,21 @@ int main(int argc, char ** argv) cm->write(cm->now(), measured_period); // wait until we hit the end of the period - next_iteration_time += period; if (use_sim_time) { cm->get_clock()->sleep_until(current_time + period); } else { - const auto time_now = cm->now().nanoseconds(); - if (next_iteration_time.time_since_epoch().count() < time_now) + next_iteration_time += period; + const auto time_now = std::chrono::steady_clock::now(); + if (next_iteration_time < time_now) { const double time_diff = - static_cast((time_now - next_iteration_time.time_since_epoch().count())) / 1.e6; + static_cast( + std::chrono::duration_cast(time_now - next_iteration_time) + .count()) / + 1.e6; const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); RCLCPP_WARN( From c37fd3aed869030f19a5c00038400815b34d27ea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 7 Feb 2025 00:28:13 +0100 Subject: [PATCH 09/12] Use clock type for the print instead of has_realtime --- controller_manager/src/ros2_control_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 2550bfabf6..ced5ce58cc 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -61,7 +61,7 @@ int main(int argc, char ** argv) executor, manager_node_name, "", cm_node_options); RCLCPP_INFO( cm->get_logger(), "Starting controller manager using %s clock", - has_realtime ? "STEADY" : "ROS"); + cm->get_clock()->get_clock_type() == RCL_STEADY_TIME ? "STEADY" : "ROS"); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); From 4ac2cf34d721d0aa1dbc08ff67566847e8aaca7a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 7 Feb 2025 11:35:25 +0100 Subject: [PATCH 10/12] make the fix work for Humble --- controller_manager/src/ros2_control_node.cpp | 26 +++++++++++--------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index ced5ce58cc..0d878a0d36 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -20,6 +20,7 @@ #include "controller_manager/controller_manager.hpp" #include "rclcpp/executors.hpp" +#include "rclcpp/version.h" #include "realtime_tools/realtime_helpers.hpp" using namespace std::chrono_literals; @@ -55,13 +56,16 @@ int main(int argc, char ** argv) cm_node_options.arguments(node_arguments); const bool has_realtime = realtime_tools::has_realtime_kernel(); const auto cm_clock_type = has_realtime ? RCL_STEADY_TIME : RCL_ROS_TIME; +#if RCLCPP_VERSION_MAJOR >= 18 cm_node_options.clock_type(cm_clock_type); - +#endif auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const auto cm_clock = + (RCLCPP_VERSION_MAJOR >= 18) ? cm->get_clock() : std::make_shared(cm_clock_type); RCLCPP_INFO( cm->get_logger(), "Starting controller manager using %s clock", - cm->get_clock()->get_clock_type() == RCL_STEADY_TIME ? "STEADY" : "ROS"); + cm_clock->get_clock_type() == RCL_STEADY_TIME ? "STEADY" : "ROS"); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); @@ -99,8 +103,8 @@ 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())); + cm_clock->wait_until_started(); + cm_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); @@ -109,7 +113,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time]() + [cm, cm_clock, thread_priority, use_sim_time]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -131,7 +135,7 @@ int main(int argc, char ** argv) auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now(); + rclcpp::Time previous_time = cm_clock->now(); std::this_thread::sleep_for(period); std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; @@ -139,19 +143,19 @@ int main(int argc, char ** argv) while (rclcpp::ok()) { // calculate measured period - auto const current_time = cm->now(); + auto const current_time = cm_clock->now(); auto const measured_period = current_time - previous_time; previous_time = current_time; // execute update loop - cm->read(cm->now(), measured_period); - cm->update(cm->now(), measured_period); - cm->write(cm->now(), measured_period); + cm->read(cm_clock->now(), measured_period); + cm->update(cm_clock->now(), measured_period); + cm->write(cm_clock->now(), measured_period); // wait until we hit the end of the period if (use_sim_time) { - cm->get_clock()->sleep_until(current_time + period); + cm_clock->sleep_until(current_time + period); } else { From 42d42b1ce87b4ae1a51e7328ea477d147953b5d0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 7 Feb 2025 11:49:32 +0100 Subject: [PATCH 11/12] revert the cm_clock variable changes --- controller_manager/src/ros2_control_node.cpp | 22 +++++++++----------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 0d878a0d36..253a22fcbd 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -61,11 +61,9 @@ int main(int argc, char ** argv) #endif auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); - const auto cm_clock = - (RCLCPP_VERSION_MAJOR >= 18) ? cm->get_clock() : std::make_shared(cm_clock_type); RCLCPP_INFO( cm->get_logger(), "Starting controller manager using %s clock", - cm_clock->get_clock_type() == RCL_STEADY_TIME ? "STEADY" : "ROS"); + cm->get_clock()->get_clock_type() == RCL_STEADY_TIME ? "STEADY" : "ROS"); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); @@ -103,8 +101,8 @@ int main(int argc, char ** argv) } // wait for the clock to be available - cm_clock->wait_until_started(); - cm_clock->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); + 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); @@ -113,7 +111,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, cm_clock, thread_priority, use_sim_time]() + [cm, thread_priority, use_sim_time]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -135,7 +133,7 @@ int main(int argc, char ** argv) auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); // for calculating the measured period of the loop - rclcpp::Time previous_time = cm_clock->now(); + rclcpp::Time previous_time = cm->now(); std::this_thread::sleep_for(period); std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; @@ -143,19 +141,19 @@ int main(int argc, char ** argv) while (rclcpp::ok()) { // calculate measured period - auto const current_time = cm_clock->now(); + auto const current_time = cm->now(); auto const measured_period = current_time - previous_time; previous_time = current_time; // execute update loop - cm->read(cm_clock->now(), measured_period); - cm->update(cm_clock->now(), measured_period); - cm->write(cm_clock->now(), measured_period); + cm->read(cm->now(), measured_period); + cm->update(cm->now(), measured_period); + cm->write(cm->now(), measured_period); // wait until we hit the end of the period if (use_sim_time) { - cm_clock->sleep_until(current_time + period); + cm->get_clock()->sleep_until(current_time + period); } else { From 322485008e7344cfac4e5be6820e48621de93e79 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 7 Feb 2025 13:09:13 +0100 Subject: [PATCH 12/12] revert the changes of the Steady clock --- controller_manager/src/ros2_control_node.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 253a22fcbd..6951a019e0 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -20,7 +20,6 @@ #include "controller_manager/controller_manager.hpp" #include "rclcpp/executors.hpp" -#include "rclcpp/version.h" #include "realtime_tools/realtime_helpers.hpp" using namespace std::chrono_literals; @@ -54,19 +53,13 @@ int main(int argc, char ** argv) node_arguments.push_back(argv[i]); } cm_node_options.arguments(node_arguments); - const bool has_realtime = realtime_tools::has_realtime_kernel(); - const auto cm_clock_type = has_realtime ? RCL_STEADY_TIME : RCL_ROS_TIME; -#if RCLCPP_VERSION_MAJOR >= 18 - cm_node_options.clock_type(cm_clock_type); -#endif + auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); - RCLCPP_INFO( - cm->get_logger(), "Starting controller manager using %s clock", - cm->get_clock()->get_clock_type() == RCL_STEADY_TIME ? "STEADY" : "ROS"); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + const bool has_realtime = realtime_tools::has_realtime_kernel(); const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); if (lock_memory) {