From a2527b7f70b4b7276dbcf20b6b6657a19c500970 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 20:10:39 +0100 Subject: [PATCH 1/3] [ros2_control_node] Handle simulation environment clocks (#1810) (cherry picked from commit d714e8bfe521c653d82e60ec2b47e5a0d5083863) # Conflicts: # doc/release_notes.rst --- controller_manager/src/ros2_control_node.cpp | 14 ++++++- doc/release_notes.rst | 44 ++++++++++++++++++++ 2 files changed, 56 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 904f9af08e..863d1cc308 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -44,6 +44,9 @@ int main(int argc, char ** argv) auto cm = std::make_shared(executor, manager_node_name); + const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock()); + const bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; if (lock_memory && !realtime_tools::lock_memory(message)) @@ -69,7 +72,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority]() + [cm, thread_priority, use_sim_time, &rate]() { if (realtime_tools::has_realtime_kernel()) { @@ -121,7 +124,14 @@ int main(int argc, char ** argv) // wait until we hit the end of the period next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); + if (use_sim_time) + { + rate.sleep(); + } + else + { + std::this_thread::sleep_until(next_iteration_time); + } } }); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index f9e1dcc858..c9ddcf5261 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -17,6 +17,50 @@ For details see the controller_manager section. controller_manager ****************** +<<<<<<< HEAD +======= + * Configured chainable controller: Listed exported interfaces are unavailable and unclaimed + * Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed + * Active chainable controller (in chained mode): Listed exported interfaces are available and claimed +* Try using SCHED_FIFO on any kernel (`#1142 `_) +* A method to get node options to setup the controller node was added (`#1169 `_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options +* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). +* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_) +* Changes from `(PR #1256) `__ + + * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: + + The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. + + This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. + + * The syntax for mimic joints is changed to the `official URDF specification `__. + + .. code-block:: xml + + + + + + + + + + + + + + + + + + The parameters within the ``ros2_control`` tag are not supported any more. +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). +* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). +* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). +* Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). +>>>>>>> d714e8b ([ros2_control_node] Handle simulation environment clocks (#1810)) * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). From 7a69f41cae68f659542668637e9f4940976f4f41 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 7 Nov 2024 16:19:16 +0100 Subject: [PATCH 2/3] Update release_notes.rst --- doc/release_notes.rst | 43 ------------------------------------------- 1 file changed, 43 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index c9ddcf5261..06e1d8543e 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -17,50 +17,7 @@ For details see the controller_manager section. controller_manager ****************** -<<<<<<< HEAD -======= - * Configured chainable controller: Listed exported interfaces are unavailable and unclaimed - * Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed - * Active chainable controller (in chained mode): Listed exported interfaces are available and claimed -* Try using SCHED_FIFO on any kernel (`#1142 `_) -* A method to get node options to setup the controller node was added (`#1169 `_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options -* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). -* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_) -* Changes from `(PR #1256) `__ - - * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: - - The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. - - This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. - - * The syntax for mimic joints is changed to the `official URDF specification `__. - - .. code-block:: xml - - - - - - - - - - - - - - - - - - The parameters within the ``ros2_control`` tag are not supported any more. -* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). -* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). -* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). -* Added support for the wildcard entries for the controller configuration files (`#1724 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). ->>>>>>> d714e8b ([ros2_control_node] Handle simulation environment clocks (#1810)) * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). From f1004d17ac5963a3dd1d282eea1307bb2b841dc7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:17:52 +0100 Subject: [PATCH 3/3] Use Clock instead of Rate for backward compatibility of rolling (#1864) --- controller_manager/src/ros2_control_node.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 863d1cc308..9c0d304e1a 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -45,7 +45,6 @@ int main(int argc, char ** argv) auto cm = std::make_shared(executor, manager_node_name); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); - rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock()); const bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; @@ -72,7 +71,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time, &rate]() + [cm, thread_priority, use_sim_time]() { if (realtime_tools::has_realtime_kernel()) { @@ -126,7 +125,7 @@ int main(int argc, char ** argv) next_iteration_time += period; if (use_sim_time) { - rate.sleep(); + cm->get_clock()->sleep_until(current_time + period); } else {