From 5dbd6d55c60dd6ad8d0244dca900e9a52f5b3c25 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 31 Oct 2024 13:05:27 +0100 Subject: [PATCH 01/21] Fix Hardware spawner and add tests for it (#1759) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Tests for hardware spawner. * Fully support spawning a list of hardware components * Use python3 coverage instead of ros2 run in tests * Actually check component's state after changing it * Update hardware_spawner's user documentation --------- Co-authored-by: Dr. Denis Štogl Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- controller_manager/CMakeLists.txt | 9 + .../controller_manager/hardware_spawner.py | 70 ++--- controller_manager/doc/userdoc.rst | 10 +- .../test/test_hardware_spawner.cpp | 282 ++++++++++++++++++ 4 files changed, 333 insertions(+), 38 deletions(-) create mode 100644 controller_manager/test/test_hardware_spawner.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e44fb5fb9b..a1a98bc59a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -194,6 +194,15 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + ament_add_gmock(test_hardware_spawner + test/test_hardware_spawner + TIMEOUT 120 + ) + target_link_libraries(test_hardware_spawner + controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + install(FILES test/test_controller_spawner_with_fallback_controllers.yaml DESTINATION test) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 29c0b5e97c..323e02584a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names): def is_hardware_component_loaded( node, controller_manager, hardware_component, service_timeout=0.0 ): - components = list_hardware_components(node, hardware_component, service_timeout).component + components = list_hardware_components(node, controller_manager, service_timeout).component return any(c.name == hardware_component for c in components) @@ -82,34 +82,33 @@ def handle_set_component_state_service_call( ) -def activate_components(node, controller_manager_name, components_to_activate): +def activate_component(node, controller_manager_name, component_to_activate): active_state = State() active_state.id = State.PRIMARY_STATE_ACTIVE active_state.label = "active" - for component in components_to_activate: - handle_set_component_state_service_call( - node, controller_manager_name, component, active_state, "activated" - ) + handle_set_component_state_service_call( + node, controller_manager_name, component_to_activate, active_state, "activated" + ) -def configure_components(node, controller_manager_name, components_to_configure): +def configure_component(node, controller_manager_name, component_to_configure): inactive_state = State() inactive_state.id = State.PRIMARY_STATE_INACTIVE inactive_state.label = "inactive" - for component in components_to_configure: - handle_set_component_state_service_call( - node, controller_manager_name, component, inactive_state, "configured" - ) + handle_set_component_state_service_call( + node, controller_manager_name, component_to_configure, inactive_state, "configured" + ) def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() - activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True) + activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True) parser.add_argument( - "hardware_component_name", - help="The name of the hardware component which should be activated.", + "hardware_component_names", + help="The name of the hardware components which should be activated.", + nargs="+", ) parser.add_argument( "-c", @@ -126,13 +125,13 @@ def main(args=None): type=float, ) # add arguments which are mutually exclusive - activate_or_confiigure_grp.add_argument( + activate_or_configure_grp.add_argument( "--activate", help="Activates the given components. Note: Components are by default configured before activated. ", action="store_true", required=False, ) - activate_or_confiigure_grp.add_argument( + activate_or_configure_grp.add_argument( "--configure", help="Configures the given components.", action="store_true", @@ -141,9 +140,9 @@ def main(args=None): command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) + hardware_components = args.hardware_component_names controller_manager_name = args.controller_manager controller_manager_timeout = args.controller_manager_timeout - hardware_component = [args.hardware_component_name] activate = args.activate configure = args.configure @@ -156,24 +155,25 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not is_hardware_component_loaded( - node, controller_manager_name, hardware_component, controller_manager_timeout - ): - node.get_logger().warn( - bcolors.WARNING - + "Hardware Component is not loaded - state can not be changed." - + bcolors.ENDC - ) - elif activate: - activate_components(node, controller_manager_name, hardware_component) - elif configure: - configure_components(node, controller_manager_name, hardware_component) - else: - node.get_logger().error( - 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' - ) - parser.print_help() - return 0 + for hardware_component in hardware_components: + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout + ): + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: + activate_component(node, controller_manager_name, hardware_component) + elif configure: + configure_component(node, controller_manager_name, hardware_component) + else: + node.get_logger().error( + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + ) + parser.print_help() + return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 312e7c68ef..fa673cff0a 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -259,16 +259,20 @@ The parsed controller config file can follow the same conventions as the typical .. code-block:: console $ ros2 run controller_manager hardware_spawner -h - usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + (--activate | --configure) + hardware_component_names [hardware_component_names ...] positional arguments: - hardware_component_name - The name of the hardware component which should be activated. + hardware_component_names + The name of the hardware components which should be activated. options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node + --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT + Time to wait for the controller manager --activate Activates the given components. Note: Components are by default configured before activated. --configure Configures the given components. diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp new file mode 100644 index 0000000000..6fd1269fa9 --- /dev/null +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -0,0 +1,282 @@ +// Copyright 2021 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +class RMServiceCaller +{ +public: + explicit RMServiceCaller(const std::string & cm_name) + { + list_srv_node_ = std::make_shared("list_srv_client"); + srv_executor_.add_node(list_srv_node_); + list_hw_components_client_ = + list_srv_node_->create_client( + cm_name + "/list_hardware_components"); + } + + lifecycle_msgs::msg::State get_component_state(const std::string & component_name) + { + auto request = + std::make_shared(); + EXPECT_TRUE(list_hw_components_client_->wait_for_service(std::chrono::milliseconds(500))); + auto future = list_hw_components_client_->async_send_request(request); + EXPECT_EQ(srv_executor_.spin_until_future_complete(future), rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + + auto it = std::find_if( + std::begin(result->component), std::end(result->component), + [&component_name](const auto & cmp) { return cmp.name == component_name; }); + + EXPECT_NE(it, std::end(result->component)); + + return it->state; + }; + +protected: + rclcpp::executors::SingleThreadedExecutor srv_executor_; + rclcpp::Node::SharedPtr list_srv_node_; + rclcpp::Client::SharedPtr + list_hw_components_client_; +}; + +using namespace std::chrono_literals; +class TestHardwareSpawner : public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawner() + : ControllerManagerFixture(), RMServiceCaller(TEST_CM_NAME) + { + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +int call_spawner(const std::string extra_args) +{ + std::string spawner_script = + "python3 -m coverage run --append --branch $(ros2 pkg prefix " + "controller_manager)/lib/controller_manager/hardware_spawner "; + return std::system((spawner_script + extra_args).c_str()); +} + +TEST_F(TestHardwareSpawner, spawner_with_no_arguments_errors) +{ + EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; +} + +TEST_F(TestHardwareSpawner, spawner_without_manager_errors_with_given_timeout) +{ + EXPECT_NE(call_spawner("TestSystemHardware --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; +} + +TEST_F(TestHardwareSpawner, spawner_without_component_name_argument) +{ + EXPECT_NE(call_spawner("-c test_controller_manager"), 0) + << "Missing component name argument parameter"; +} + +TEST_F(TestHardwareSpawner, spawner_non_exising_hardware_component) +{ + EXPECT_NE(call_spawner("TestSystemHardware1 -c test_controller_manager"), 0) + << "Missing component name argument parameter"; +} + +TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activated) +{ + EXPECT_EQ(call_spawner("TestSystemHardware --configure -c test_controller_manager"), 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + EXPECT_EQ(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +} + +class TestHardwareSpawnerWithoutRobotDescription +: public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawnerWithoutRobotDescription() + : ControllerManagerFixture(""), + RMServiceCaller(TEST_CM_NAME) + { + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.unconfigured", + std::vector{"TestSystemHardware"})); + } + +public: + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestHardwareSpawnerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "could not change hardware state because not robot description and not services for " + "controller " + "manager are active"; +} + +TEST_F(TestHardwareSpawnerWithoutRobotDescription, spawner_with_later_load_of_robot_description) +{ + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "could not activate control because not robot description"; + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 2.5"), + 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} + +class TestHardwareSpawnerWithNamespacedCM +: public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawnerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace"), + RMServiceCaller("foo_namespace/" + std::string(TEST_CM_NAME)) + { + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + } + +public: + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm_namespace) +{ + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "Should fail without defining the namespace"; + EXPECT_EQ( + call_spawner("TestSystemHardware --configure -c test_controller_manager --ros-args -r " + "__ns:=/foo_namespace"), + 0); + + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} From 0be43748de967d1d410385411baf45107145d24d Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Fri, 1 Nov 2024 09:50:10 +0100 Subject: [PATCH 02/21] Bump version of pre-commit hooks (#1831) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63e7f08682..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.17.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,7 +50,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=99"] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.0 + rev: v19.1.3 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.3 + rev: 0.29.4 hooks: - id: check-github-workflows args: ["--verbose"] From 58805d35b75a19ff289c2dfd1582c8a353368980 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 3 Nov 2024 18:05:36 +0100 Subject: [PATCH 03/21] Update mergify.yml author entries (#1841) --- .github/mergify.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 39ee6b6bc0..fd185e02d0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -32,7 +32,7 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - name: development targets master branch conditions: @@ -40,6 +40,7 @@ pull_request_rules: - author!=bmagyar - author!=destogl - author!=christophfroehlich + - author!=saikishor - author!=mergify[bot] - author!=dependabot[bot] actions: From be0a339ac447cf259b560f13f634dd0ced6028ff Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Mon, 4 Nov 2024 17:18:19 +0100 Subject: [PATCH 04/21] fix: typo use thread_priority (#1844) --- 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 13af864c2f..c0a61d6cda 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -79,7 +79,7 @@ int main(int argc, char ** argv) { RCLCPP_INFO( cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", - kSchedPriority); + thread_priority); } // for calculating sleep time From 434f7f54c91114028b8f73c927ecee41588f2bce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 4 Nov 2024 19:44:34 +0100 Subject: [PATCH 05/21] Fix CMP0115 (#1830) --- controller_manager/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index a1a98bc59a..6e004ffc4a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -195,7 +195,7 @@ if(BUILD_TESTING) ) ament_add_gmock(test_hardware_spawner - test/test_hardware_spawner + test/test_hardware_spawner.cpp TIMEOUT 120 ) target_link_libraries(test_hardware_spawner From e6e69c1052b91d52de792f49d89c6c54bf4ec18a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 20:53:55 +0100 Subject: [PATCH 06/21] [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (#1822) --- controller_manager/src/ros2_control_node.cpp | 7 +++++++ doc/release_notes.rst | 1 + 2 files changed, 8 insertions(+) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index c0a61d6cda..6de49166a9 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,6 +57,13 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const bool lock_memory = cm->get_parameter_or("lock_memory", true); + std::string message; + if (lock_memory && !realtime_tools::lock_memory(message)) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + } + 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( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a7c1242ffc..9b70016184 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -77,6 +77,7 @@ controller_manager * 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 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). +* 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 `_). hardware_interface ****************** From 98f079598a6444d7043236c2e9fec04d1f7dae02 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 Nov 2024 11:08:17 +0100 Subject: [PATCH 07/21] [ros2_control_node] Add option to set the CPU affinity (#1852) --- controller_manager/src/ros2_control_node.cpp | 11 +++++++++++ doc/release_notes.rst | 1 + 2 files changed, 12 insertions(+) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6de49166a9..51e7e83d23 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -64,6 +64,17 @@ int main(int argc, char ** argv) RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); } + const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1); + if (cpu_affinity >= 0) + { + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity); + if (!affinity_result.first) + { + RCLCPP_WARN( + cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); + } + } + 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( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9b70016184..12ded009dc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -78,6 +78,7 @@ controller_manager * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * 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 `_). +* The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). hardware_interface ****************** From 814137dd3a87ac21c70c84420c985e436e1f5f7c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 Nov 2024 13:40:17 +0100 Subject: [PATCH 08/21] [CM] Fix controller missing update cycles in a real setup (#1774) --- .../controller_interface_base.hpp | 2 +- controller_manager/CMakeLists.txt | 1 + .../controller_manager/controller_manager.hpp | 2 +- .../controller_manager/controller_spec.hpp | 2 +- controller_manager/src/controller_manager.cpp | 34 ++++---- .../test/test_controller_manager.cpp | 77 ++++++++++--------- 6 files changed, 66 insertions(+), 52 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 211716f301..ae3804919c 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -160,7 +160,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration + * \param[in] period The measured time since the last control loop iteration * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. */ CONTROLLER_INTERFACE_PUBLIC diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 6e004ffc4a..1bb84eb32c 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -87,6 +87,7 @@ if(BUILD_TESTING) ament_add_gmock(test_controller_manager test/test_controller_manager.cpp + TIMEOUT 180 ) target_link_libraries(test_controller_manager controller_manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 6c0b4fde9b..068eefc1f9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -123,7 +123,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9ce1aab8b6..0f44867814 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -37,7 +37,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; - std::shared_ptr next_update_cycle_time; + std::shared_ptr last_update_cycle_time; }; struct ControllerChainSpec diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a8668f7f1b..d7e924a863 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -543,7 +543,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared( + controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); // We have to fetch the parameters_file at the time of loading the controller, because this way we @@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers( continue; } auto controller = found_it->c; - // reset the next update cycle time for newly activated controllers - *found_it->next_update_cycle_time = + // reset the last update cycle time for newly activated controllers + *found_it->last_update_cycle_time = rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; @@ -2354,21 +2354,31 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + const rclcpp::Time current_time = get_clock()->now(); if ( - *loaded_controller.next_update_cycle_time == + *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { // it is zero after activation + *loaded_controller.last_update_cycle_time = current_time - controller_period; RCLCPP_DEBUG( - get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s", - time.seconds(), loaded_controller.info.name.c_str()); - *loaded_controller.next_update_cycle_time = time; + get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", + loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } - - bool controller_go = + const auto controller_actual_period = + (current_time - *loaded_controller.last_update_cycle_time); + + /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the + /// jitter in the system sleep cycles. + // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have + // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we + // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue + // to keep up with the controller update rate (see issue #1769). + const bool controller_go = + run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); + (controller_actual_period.seconds() * controller_update_rate >= 0.99); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2377,8 +2387,6 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - const auto controller_actual_period = - (time - *loaded_controller.next_update_cycle_time) + controller_period; auto controller_ret = controller_interface::return_type::OK; bool trigger_status = true; // Catch exceptions thrown by the controller update function @@ -2402,7 +2410,7 @@ controller_interface::return_type ControllerManager::update( controller_ret = controller_interface::return_type::ERROR; } - *loaded_controller.next_update_cycle_time += controller_period; + *loaded_controller.last_update_cycle_time = current_time; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 8ff844adc2..0c4e51985f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -575,10 +575,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // In case of a non perfect divisor, the update period should respect the rule // [cm_update_rate, 2*cm_update_rate) EXPECT_THAT( - test_controller->update_period_, - testing::AllOf( - testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)), - testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate)))); + test_controller->update_period_.seconds(), + testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -640,6 +638,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ControllerManagerRunner cm_runner(this); cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); } + time_ = test_controller->get_node()->now(); // set to something nonzero + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -650,7 +651,6 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function - time_ = test_controller->get_node()->now(); // set to something nonzero const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; std::vector stop_controllers = {}; @@ -661,7 +661,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - time_ += rclcpp::Duration::from_seconds(0.01); + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -677,25 +678,29 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); const auto controller_update_rate = test_controller->get_update_rate(); + const double controller_period = 1.0 / controller_update_rate; const auto initial_counter = test_controller->internal_counter; // don't start with zero to check if the period is correct if controller is activated anytime rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { + rclcpp::Time old_time = time; + cm_->get_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); // In case of a non perfect divisor, the update period should respect the rule // [controller_update_rate, 2*controller_update_rate) EXPECT_THAT( - test_controller->update_period_, + test_controller->update_period_.seconds(), testing::AllOf( - testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)), - testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate)))) - << "update_counter: " << update_counter; + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); - time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; @@ -708,15 +713,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_THAT( test_controller->internal_counter - initial_counter, testing::AnyOf( - testing::Eq(controller_update_rate * no_of_secs_passed), - testing::Eq((controller_update_rate * no_of_secs_passed) - 1))); + testing::Ge((controller_update_rate - 1) * no_of_secs_passed), + testing::Lt((controller_update_rate * no_of_secs_passed)))); } } } INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, - testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90)); class TestControllerManagerFallbackControllers : public ControllerManagerFixture, @@ -764,7 +769,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -772,7 +777,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -846,7 +851,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -854,7 +859,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -944,7 +949,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -952,7 +957,7 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -1033,7 +1038,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1041,7 +1046,7 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -1129,7 +1134,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1137,14 +1142,14 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 } @@ -1279,7 +1284,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1287,21 +1292,21 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 controller_spec.c = test_controller_4; controller_spec.info.name = test_controller_4_name; controller_spec.info.type = "test_chainable_controller::TestChainableController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_4 } @@ -1381,7 +1386,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( @@ -1413,7 +1418,7 @@ TEST_F( // available controller_spec.info.fallback_controllers_names = { test_controller_4_name, test_controller_3_name, test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( @@ -1509,7 +1514,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_4_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1517,21 +1522,21 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 controller_spec.c = test_controller_4; controller_spec.info.name = test_controller_4_name; controller_spec.info.type = "test_chainable_controller::TestChainableController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_4 } @@ -1617,7 +1622,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_3_name, test_controller_4_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( From 033a6bcd5ece6a34140106f2ebadeafa2e932376 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 19:46:51 +0100 Subject: [PATCH 09/21] [HW_IF] Prepare the handles for async operations (#1750) --- .../include/hardware_interface/handle.hpp | 85 +++++++++++++++++-- .../loaned_command_interface.hpp | 85 ++++++++++++++++++- .../loaned_state_interface.hpp | 56 +++++++++++- 3 files changed, 217 insertions(+), 9 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6fe4f25663..1dfd499c2c 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,7 +17,10 @@ #include #include +#include +#include #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -69,13 +72,57 @@ class Handle { } - Handle(const Handle & other) = default; + Handle(const Handle & other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } - Handle(Handle && other) = default; + Handle(Handle && other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } - Handle & operator=(const Handle & other) = default; + Handle & operator=(const Handle & other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } + return *this; + } - Handle & operator=(Handle && other) = default; + Handle & operator=(Handle && other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } + return *this; + } virtual ~Handle() = default; @@ -95,8 +142,14 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } + [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]] double get_value() const { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return std::numeric_limits::quiet_NaN(); + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); @@ -104,12 +157,33 @@ class Handle // END } - void set_value(double value) + [[nodiscard]] bool get_value(double & value) const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value directly if old functionality is removed + THROW_ON_NULLPTR(value_ptr_); + value = *value_ptr_; + return true; + // END + } + + [[nodiscard]] bool set_value(double value) { + std::unique_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) set value_ directly if old functionality is removed THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; + return true; // END } @@ -122,6 +196,7 @@ class Handle // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; // END + mutable std::shared_mutex handle_mutex_; }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index aa306870a1..6013dea778 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,10 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ #include +#include #include +#include #include #include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" namespace hardware_interface { @@ -51,6 +54,27 @@ class LoanedCommandInterface virtual ~LoanedCommandInterface() { + auto logger = rclcpp::get_logger(command_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " + "%u get_value calls", + get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " + "%u set_value calls", + get_name().c_str(), set_value_statistics_.timeout_counter, + (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.failed_counter, + (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter, + set_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -70,13 +94,70 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + template + [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10) + { + unsigned int nr_tries = 0; + ++set_value_statistics_.total_counter; + while (!command_interface_.set_value(value)) + { + ++set_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++set_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } - double get_value() const { return command_interface_.get_value(); } + double get_value() const + { + double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + ++get_value_statistics_.total_counter; + while (!command_interface_.get_value(value)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: CommandInterface & command_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; + HandleRTStatistics set_value_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 96cc3e89df..3ebc8c7ca0 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,11 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ #include +#include #include +#include #include #include "hardware_interface/handle.hpp" - +#include "rclcpp/logging.hpp" namespace hardware_interface { class LoanedStateInterface @@ -56,6 +58,17 @@ class LoanedStateInterface virtual ~LoanedStateInterface() { + auto logger = rclcpp::get_logger(state_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + logger, + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u " + "get_value calls", + state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -75,11 +88,50 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const + { + double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + ++get_value_statistics_.total_counter; + while (!state_interface_.get_value(value)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: const StateInterface & state_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; }; } // namespace hardware_interface From d714e8bfe521c653d82e60ec2b47e5a0d5083863 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 20:10:39 +0100 Subject: [PATCH 10/21] [ros2_control_node] Handle simulation environment clocks (#1810) --- controller_manager/src/ros2_control_node.cpp | 14 ++++++++++++-- doc/release_notes.rst | 1 + 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 51e7e83d23..0ed68d9765 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,6 +57,9 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + 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)) @@ -82,7 +85,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::configure_sched_fifo(thread_priority)) { @@ -123,7 +126,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); + } } cm->shutdown_async_controllers_and_components(); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 12ded009dc..b11a1351fe 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -76,6 +76,7 @@ controller_manager * 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 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * 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 `_). * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). From 93a2a68f6ab4110b7e5816bce134437780ba3bf3 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Fri, 8 Nov 2024 00:53:02 +0530 Subject: [PATCH 11/21] Add Support for SDF (#1763) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Signed-off-by: Aarav Gupta <134804732+Amronos@users.noreply.github.com> Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar --- doc/release_notes.rst | 1 + hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 21 +- .../test/test_component_parser.cpp | 35 +++ .../ros2_control_test_assets/descriptions.hpp | 219 ++++++++++++++++++ 5 files changed, 273 insertions(+), 4 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b11a1351fe..a6ebb16e5f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -113,6 +113,7 @@ hardware_interface * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. +* With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. joint_limits ************ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 96d593e8f6..05cff62957 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -18,6 +18,7 @@ tinyxml2_vendor joint_limits urdf + sdformat_urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d2ec0f9d53..a49b83b964 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -31,6 +31,8 @@ namespace { constexpr const auto kRobotTag = "robot"; +constexpr const auto kSDFTag = "sdf"; +constexpr const auto kModelTag = "model"; constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; constexpr const auto kPluginNameTag = "plugin"; @@ -812,15 +814,26 @@ std::vector parse_control_resources_from_urdf(const std::string & "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); } - // Find robot tag + // Find robot or sdf tag const tinyxml2::XMLElement * robot_it = doc.RootElement(); + const tinyxml2::XMLElement * ros2_control_it; - if (std::string(kRobotTag) != robot_it->Name()) + if (std::string(kRobotTag) == robot_it->Name()) { - throw std::runtime_error("the robot tag is not root element in URDF"); + ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); + } + else if (std::string(kSDFTag) == robot_it->Name()) + { + // find model tag in sdf tag + const tinyxml2::XMLElement * model_it = robot_it->FirstChildElement(kModelTag); + ros2_control_it = model_it->FirstChildElement(kROS2ControlTag); + } + else + { + throw std::runtime_error( + "the robot tag is not root element in URDF or sdf tag is not root element in SDF"); } - const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); if (!ros2_control_it) { throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 20e098b62e..3c24b0cc2a 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1616,3 +1616,38 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); } + +TEST_F(TestComponentParser, successfully_parse_valid_sdf) +{ + std::string sdf_to_test = ros2_control_test_assets::diff_drive_robot_sdf; + const auto control_hardware = parse_control_resources_from_urdf(sdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "GazeboSimSystem"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ(hardware_info.hardware_plugin_name, "gz_ros2_control/GazeboSimSystem"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "left_wheel_joint"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-10"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "10"); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_POSITION); + + EXPECT_EQ(hardware_info.joints[1].name, "right_wheel_joint"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-10"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "10"); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_POSITION); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index cc2b1798d4..e94d4e6736 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -1715,6 +1715,225 @@ const auto gripper_hardware_resources_mimic_false_command_if = )"; +const auto diff_drive_robot_sdf = + R"( + + + + + + true + + + + base_link + chassis_link + 0 0 0.075 0 0 0 + + + + + + + + 0.3 0.3 0.15 + + + + + 1 1 1 1 + 1 1 1 1 + + + + + + + 0.3 0.3 0.15 + + + + + + 0.5 + + 0.0046875 + 0.0 + 0.0 + 0.0046875 + 0.0 + 0.0075 + + + + + + chassis_link + left_wheel_link + 0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + right_wheel_link + 0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + caster_link + -0.09 0 -0.075 0 0 0 + + 1 1 1 + + -inf + inf + + + + + + + + + 0.05 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + + + + + 0.1 + + 0.00010000000000000002 + 0.0 + 0.0 + 0.00010000000000000002 + 0.0 + 0.00010000000000000002 + + + + + + gz_ros2_control/GazeboSimSystem + + + + -10 + 10 + + + + + + + -10 + 10 + + + + + + + /path/to/config.yml + + + +)"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = From 5d36d99c97dfb54e5265b08777931d933f64e266 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:17:52 +0100 Subject: [PATCH 12/21] 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 0ed68d9765..dcf508d13c 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -58,7 +58,6 @@ int main(int argc, char ** argv) executor, manager_node_name, "", cm_node_options); 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; @@ -85,7 +84,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::configure_sched_fifo(thread_priority)) { @@ -128,7 +127,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 { From 569fb58b8520645445fe3993e5240d3350d227b3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:18:14 +0100 Subject: [PATCH 13/21] reset the async variables upon activation to work post exceptions (#1860) --- controller_interface/src/controller_interface_base.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a6e0da988f..0713ec3c25 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -67,7 +67,15 @@ return_type ControllerInterfaceBase::init( }); node_->register_on_activate( - std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); + [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn + { + if (is_async() && async_handler_ && async_handler_->is_running()) + { + // This is needed if it is disabled due to a thrown exception in the async callback thread + async_handler_->reset_variables(); + } + return on_activate(previous_state); + }); node_->register_on_deactivate( std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1)); From 8fa60dba374563077d221d2395867ce1906c41fe Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:18:44 +0100 Subject: [PATCH 14/21] change from thread_priority.hpp to realtime_helpers.hpp (#1829) --- 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 dcf508d13c..f8347df952 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -20,7 +20,7 @@ #include "controller_manager/controller_manager.hpp" #include "rclcpp/executors.hpp" -#include "realtime_tools/thread_priority.hpp" +#include "realtime_tools/realtime_helpers.hpp" using namespace std::chrono_literals; From 8853a188813671609d017fefb8c04ff6b2246185 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 11:02:53 +0000 Subject: [PATCH 15/21] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 14 ++++++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 52 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index be2de417d6..c3f845e1ee 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* reset the async variables upon activation to work post exceptions (`#1860 `_) +* [CM] Fix controller missing update cycles in a real setup (`#1774 `_) +* Contributors: Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * [CM] Async Function Handler for Controllers (`#1489 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 7d9fb12a0d..51a4e47855 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* change from thread_priority.hpp to realtime_helpers.hpp (`#1829 `_) +* Use Clock instead of Rate for backward compatibility of rolling (`#1864 `_) +* [ros2_control_node] Handle simulation environment clocks (`#1810 `_) +* [CM] Fix controller missing update cycles in a real setup (`#1774 `_) +* [ros2_control_node] Add option to set the CPU affinity (`#1852 `_) +* [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (`#1822 `_) +* Fix CMP0115 (`#1830 `_) +* fix: typo use thread_priority (`#1844 `_) +* Fix Hardware spawner and add tests for it (`#1759 `_) +* add thread_priority option to the ros2_control_node (`#1820 `_) +* Contributors: Baris Yazici, Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * Fix timeout value in std output (`#1807 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 48802d740a..86f1bdead9 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 913d4cc36b..42e9c66514 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Support for SDF (`#1763 `_) +* [HW_IF] Prepare the handles for async operations (`#1750 `_) +* Contributors: Aarav Gupta, Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 8cab703114..900e855979 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 7c1e88b2f9..de4f2c0579 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 771506022e..280375752f 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 8a2406ca4e..e23d33f756 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Support for SDF (`#1763 `_) +* Contributors: Aarav Gupta + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 546356417a..4ad90c790c 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- * [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index da13697e81..fc1bc6fd73 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- * fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_) diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 68b6ec5054..185ab62c20 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- From 0031ada55856f61782f74796aa3ba9ccee08b392 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 11:03:36 +0000 Subject: [PATCH 16/21] 4.20.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c3f845e1ee..665b0175f6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * reset the async variables upon activation to work post exceptions (`#1860 `_) * [CM] Fix controller missing update cycles in a real setup (`#1774 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 55c47473f0..ca3fc2af49 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.19.0 + 4.20.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 51a4e47855..376b15014d 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * change from thread_priority.hpp to realtime_helpers.hpp (`#1829 `_) * Use Clock instead of Rate for backward compatibility of rolling (`#1864 `_) * [ros2_control_node] Handle simulation environment clocks (`#1810 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 69f5f6e4ce..18189d5d16 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.19.0 + 4.20.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 86f1bdead9..46ecc7ab6e 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index fbd4859323..78a9f99591 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.19.0 + 4.20.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 42e9c66514..17e276bc93 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * Add Support for SDF (`#1763 `_) * [HW_IF] Prepare the handles for async operations (`#1750 `_) * Contributors: Aarav Gupta, Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 05cff62957..dbe9ad5750 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.19.0 + 4.20.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 900e855979..8daf6bab93 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 1c4f2f1b73..3966cbc993 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.19.0 + 4.20.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index de4f2c0579..6b00437dd1 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index a945aa4710..c7fca5f2af 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.19.0 + 4.20.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 280375752f..5b57ddc2e2 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 71ec807544..925ad6e23f 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.19.0 + 4.20.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index e23d33f756..cad5325b2e 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * Add Support for SDF (`#1763 `_) * Contributors: Aarav Gupta diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 9752b87fca..bb818063ab 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.19.0 + 4.20.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 4ad90c790c..894bbb8ea3 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 67b651e3ee..ddb904c432 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.19.0 + 4.20.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 244359601e..949445ec59 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index fc1bc6fd73..1f9ad52f22 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index ca6e74373c..1611ce05a3 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.19.0 + 4.20.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 70cf0f8984..76d8c706c2 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 185ab62c20..842cc76488 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 22d0c90ba1..7ae2f2eda7 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.19.0 + 4.20.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From ec70ae1dc66ff39ce9a2fddf83a04c4261897dcb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 9 Nov 2024 09:58:23 +0100 Subject: [PATCH 17/21] Fix the missing bcolors.ENDC in hardware_spawner log prints (#1870) --- .../controller_manager/hardware_spawner.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 323e02584a..4f7afe714c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -67,18 +67,15 @@ def handle_set_component_state_service_call( response = set_hardware_component_state(node, controller_manager_name, component, target_state) if response.ok and response.state == target_state: node.get_logger().info( - bcolors.OKGREEN - + f"{action} component '{component}'. Hardware now in state: {response.state}." + f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}" ) elif response.ok and not response.state == target_state: node.get_logger().warn( - bcolors.WARNING - + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'." + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}" ) else: node.get_logger().warn( - bcolors.WARNING - + f"Could not {action} component '{component}'. Service call failed. Wrong component name?" + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}" ) @@ -160,9 +157,7 @@ def main(args=None): node, controller_manager_name, hardware_component, controller_manager_timeout ): node.get_logger().warn( - bcolors.WARNING - + "Hardware Component is not loaded - state can not be changed." - + bcolors.ENDC + f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}" ) elif activate: activate_component(node, controller_manager_name, hardware_component) @@ -170,14 +165,14 @@ def main(args=None): configure_component(node, controller_manager_name, hardware_component) else: node.get_logger().error( - 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}' ) parser.print_help() return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: - node.get_logger().fatal(str(err)) + node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}") return 1 finally: rclpy.shutdown() From 30d51cd7d04a93886cadc7d60275c9c1c529079f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 13 Nov 2024 21:37:50 +0100 Subject: [PATCH 18/21] Add compatibility build for humble+jazzy distro (#1872) --- ...ling-compatibility-humble-binary-build.yml | 47 +++++++++++++++++++ ...lling-compatibility-jazzy-binary-build.yml | 47 +++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 .github/workflows/rolling-compatibility-humble-binary-build.yml create mode 100644 .github/workflows/rolling-compatibility-jazzy-binary-build.yml diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..0c25f2313d --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,47 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [main, testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..cddb806eee --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,47 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [main, testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master From bab2d303903970f32ec530030b0cade2ca0254bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 14 Nov 2024 11:04:55 +0100 Subject: [PATCH 19/21] Reduce number of CI jobs (#1876) --- .github/workflows/humble-semi-binary-build.yml | 2 +- .github/workflows/iron-semi-binary-build.yml | 2 +- .github/workflows/jazzy-semi-binary-build.yml | 2 +- .../workflows/rolling-compatibility-humble-binary-build.yml | 5 +---- .../workflows/rolling-compatibility-jazzy-binary-build.yml | 5 +---- .github/workflows/rolling-semi-binary-build.yml | 2 +- 6 files changed, 6 insertions(+), 12 deletions(-) diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 560ac37cff..96938467f9 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 30a83e5367..c2d137bde0 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [iron] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 9634732cf9..12769eb740 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [jazzy] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 0c25f2313d..b55091521e 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -28,9 +28,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' jobs: build-on-humble: @@ -39,7 +36,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index cddb806eee..621ffb6a26 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -28,9 +28,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' jobs: build-on-jazzy: @@ -39,7 +36,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [jazzy] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 4cdb7ab585..492da45ab9 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [rolling] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} From da167e79ff90b250f3d4b9686e46a77c8b9ba4ab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 16 Nov 2024 17:07:47 +0100 Subject: [PATCH 20/21] Refactor: add parse_state_interface_descriptions and parse_command_interface_descriptions to import the components (#1768) --- .../hardware_interface/actuator_interface.hpp | 32 +---------- .../hardware_interface/component_parser.hpp | 20 +++++++ .../hardware_interface/sensor_interface.hpp | 16 +----- .../hardware_interface/system_interface.hpp | 53 ++----------------- hardware_interface/src/component_parser.cpp | 32 +++++++++++ 5 files changed, 60 insertions(+), 93 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 4cdd81b60f..8aa214e728 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -123,39 +123,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); - import_command_interface_descriptions(info_); + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and store them. - */ - virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_state_interface_descriptions) - { - joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - - /** - * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and store them. - */ - virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_command_interface_descriptions) - { - joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 2d0c067606..fc195d0a65 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -41,6 +42,16 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_state_interface_descriptions( const std::vector & component_info); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \param[out] state_interfaces_map unordered_map filled with information about hardware's + * StateInterfaces for the component which are exported + */ +HARDWARE_INTERFACE_PUBLIC +void parse_state_interface_descriptions( + const std::vector & component_info, + std::unordered_map & state_interfaces_map); + /** * \param[in] component_info information about a component (gpio, joint, sensor) * \return vector filled with information about hardware's CommandInterfaces for the component @@ -50,5 +61,14 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_command_interface_descriptions( const std::vector & component_info); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \param[out] command_interfaces_map unordered_map filled with information about hardware's + * CommandInterfaces for the component which are exported + */ +HARDWARE_INTERFACE_PUBLIC +void parse_command_interface_descriptions( + const std::vector & component_info, + std::unordered_map & command_interfaces_map); } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 50d79d1a45..74c1d04bd7 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -123,24 +123,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Sensor and store them. - */ - virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto sensor_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.sensors); - for (const auto & description : sensor_state_interface_descriptions) - { - sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 6b0652d3fe..18da0e4012 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -126,57 +126,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); - import_command_interface_descriptions(info_); + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint, GPIO, Sensor and store them. - */ - void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_state_interface_descriptions) - { - joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto sensor_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.sensors); - for (const auto & description : sensor_state_interface_descriptions) - { - sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto gpio_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.gpios); - for (const auto & description : gpio_state_interface_descriptions) - { - gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - - /** - * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and GPIO and store them. - */ - void import_command_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_command_interface_descriptions) - { - joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto gpio_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.gpios); - for (const auto & description : gpio_command_interface_descriptions) - { - gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index a49b83b964..0f186531e9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -942,6 +942,22 @@ std::vector parse_state_interface_descriptions( return component_state_interface_descriptions; } +void parse_state_interface_descriptions( + const std::vector & component_info, + std::unordered_map & state_interfaces_map) +{ + state_interfaces_map.reserve(state_interfaces_map.size() + component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + InterfaceDescription description(component.name, state_interface); + state_interfaces_map.insert(std::make_pair(description.get_name(), description)); + } + } +} + std::vector parse_command_interface_descriptions( const std::vector & component_info) { @@ -959,4 +975,20 @@ std::vector parse_command_interface_descriptions( return component_command_interface_descriptions; } +void parse_command_interface_descriptions( + const std::vector & component_info, + std::unordered_map & command_interfaces_map) +{ + command_interfaces_map.reserve(command_interfaces_map.size() + component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + InterfaceDescription description(component.name, command_interface); + command_interfaces_map.insert(std::make_pair(description.get_name(), description)); + } + } +} + } // namespace hardware_interface From 23bd1c3c06c30d706f010628d85133a7198e226d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 14:33:18 +0100 Subject: [PATCH 21/21] Add CM `switch_controller` service timeout as parameter to spawner.py (#1790) Co-authored-by: Felix Exner (fexner) Co-authored-by: Sai Kishor Kothakota --- .../controller_manager/spawner.py | 36 ++++++++++++++++--- .../controller_manager/unspawner.py | 18 +++++++++- controller_manager/doc/userdoc.rst | 21 ++++++----- doc/release_notes.rst | 1 + 4 files changed, 62 insertions(+), 14 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index d6df3be01f..7e9fe3443b 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -112,7 +112,16 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=0, + default=0.0, + type=float, + ) + parser.add_argument( + "--switch-timeout", + help="Time to wait for a successful state switch of controllers." + " Useful when switching cannot be performed immediately, e.g.," + " paused simulations at startup", + required=False, + default=5.0, type=float, ) parser.add_argument( @@ -129,6 +138,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout + switch_timeout = args.switch_timeout if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -206,7 +216,13 @@ def main(args=None): if not args.inactive and not args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], [controller_name], True, True, 5.0 + node, + controller_manager_name, + [], + [controller_name], + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( @@ -224,7 +240,13 @@ def main(args=None): if not args.inactive and args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], controller_names, True, True, 5.0 + node, + controller_manager_name, + [], + controller_names, + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( @@ -250,7 +272,13 @@ def main(args=None): node.get_logger().info("Interrupt captured, deactivating and unloading controller") # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index e42d85aee9..9e380f5086 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -36,17 +36,33 @@ def main(args=None): default="/controller_manager", required=False, ) + parser.add_argument( + "--switch-timeout", + help="Time to wait for a successful state switch of controllers." + " Useful when switching cannot be performed immediately, e.g.," + " paused simulations at startup", + required=False, + default=5.0, + type=float, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager + switch_timeout = args.switch_timeout node = Node("unspawner_" + controller_names[0]) try: # Ignore returncode, because message is already printed and we'll try to unload anyway ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + switch_timeout, ) node.get_logger().info("Deactivated controller") diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index fa673cff0a..ca222d68c0 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -157,9 +157,9 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h - usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-t CONTROLLER_TYPE] [-u] - [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - controller_name + usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] + controller_names [controller_names ...] positional arguments: controller_names List of controllers @@ -177,10 +177,10 @@ There are two scripts to interact with controller manager from launch files: -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused + simulations at startup --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether - --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...] - Fallback controllers list are activated as a fallback strategy when the spawned controllers fail. When the argument is provided, it takes precedence over the fallback_controllers list in the - param file The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: @@ -243,15 +243,18 @@ The parsed controller config file can follow the same conventions as the typical .. code-block:: console $ ros2 run controller_manager unspawner -h - usage: unspawner [-h] [-c CONTROLLER_MANAGER] controller_name + usage: unspawner [-h] [-c CONTROLLER_MANAGER] [--switch-timeout SWITCH_TIMEOUT] controller_names [controller_names ...] positional arguments: - controller_name Name of the controller + controller_names Name of the controller - optional arguments: + options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused + simulations at startup ``hardware_spawner`` ^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a6ebb16e5f..09a9236d79 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -76,6 +76,7 @@ controller_manager * 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 `_). +* ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#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 `_). * 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 `_).