From 10caa58b7fc18a332341725c79591c9b6065562b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Dec 2024 21:49:16 +0100 Subject: [PATCH 1/5] [CI] Add downstream builds (#1929) --- .../humble-semi-binary-downstream-build.yml | 49 ++++++++++++++++ .../jazzy-semi-binary-downstream-build.yml | 49 ++++++++++++++++ .../rolling-semi-binary-downstream-build.yml | 57 +++++++++++++++++++ downstream.humble.repos | 13 +++++ downstream.jazzy.repos | 13 +++++ downstream.rolling.repos | 13 +++++ ros_controls.humble.repos | 17 ++++++ ros_controls.jazzy.repos | 13 +++++ ros_controls.rolling.repos | 13 +++++ 9 files changed, 237 insertions(+) create mode 100644 .github/workflows/humble-semi-binary-downstream-build.yml create mode 100644 .github/workflows/jazzy-semi-binary-downstream-build.yml create mode 100644 .github/workflows/rolling-semi-binary-downstream-build.yml create mode 100644 downstream.humble.repos create mode 100644 downstream.jazzy.repos create mode 100644 downstream.rolling.repos create mode 100644 ros_controls.humble.repos create mode 100644 ros_controls.jazzy.repos create mode 100644 ros_controls.rolling.repos diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..f0bc3a9fd2 --- /dev/null +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -0,0 +1,49 @@ +name: Humble Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.humble.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_control.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.humble.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_control.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.humble.repos + not_test_downstream: true diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..3bcf417887 --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -0,0 +1,49 @@ +name: Jazzy Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.jazzy.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.jazzy.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.jazzy.repos + not_test_downstream: true diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..57db2ae7ba --- /dev/null +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -0,0 +1,57 @@ +name: Rolling Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.rolling.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: true diff --git a/downstream.humble.repos b/downstream.humble.repos new file mode 100644 index 0000000000..d4ee6ec81a --- /dev/null +++ b/downstream.humble.repos @@ -0,0 +1,13 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: humble + webots/webots_ros2: + type: git + url: https://github.com/cyberbotics/webots_ros2.git + version: master + PickNikRobotics/topic_based_ros2_control: + type: git + url: https://github.com/PickNikRobotics/topic_based_ros2_control.git + version: main diff --git a/downstream.jazzy.repos b/downstream.jazzy.repos new file mode 100644 index 0000000000..13f06e7b44 --- /dev/null +++ b/downstream.jazzy.repos @@ -0,0 +1,13 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main + webots/webots_ros2: + type: git + url: https://github.com/cyberbotics/webots_ros2.git + version: master + PickNikRobotics/topic_based_ros2_control: + type: git + url: https://github.com/PickNikRobotics/topic_based_ros2_control.git + version: main diff --git a/downstream.rolling.repos b/downstream.rolling.repos new file mode 100644 index 0000000000..13f06e7b44 --- /dev/null +++ b/downstream.rolling.repos @@ -0,0 +1,13 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main + webots/webots_ros2: + type: git + url: https://github.com/cyberbotics/webots_ros2.git + version: master + PickNikRobotics/topic_based_ros2_control: + type: git + url: https://github.com/PickNikRobotics/topic_based_ros2_control.git + version: main diff --git a/ros_controls.humble.repos b/ros_controls.humble.repos new file mode 100644 index 0000000000..8cb447612b --- /dev/null +++ b/ros_controls.humble.repos @@ -0,0 +1,17 @@ +repositories: + ros-controls/gazebo_ros2_control: + type: git + url: https://github.com/ros-controls/gazebo_ros2_control.git + version: humble + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: humble + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: humble + ros-controls/ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: humble diff --git a/ros_controls.jazzy.repos b/ros_controls.jazzy.repos new file mode 100644 index 0000000000..d2c3a15743 --- /dev/null +++ b/ros_controls.jazzy.repos @@ -0,0 +1,13 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: jazzy + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master + ros-controls/ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: master diff --git a/ros_controls.rolling.repos b/ros_controls.rolling.repos new file mode 100644 index 0000000000..45d6fe6040 --- /dev/null +++ b/ros_controls.rolling.repos @@ -0,0 +1,13 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: rolling + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master + ros-controls/ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: master From b039baa7675c7802400dd5297f839ed05435ace5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 17 Dec 2024 17:04:20 +0100 Subject: [PATCH 2/5] Use singleton approach to store and reuse the service clients (#1949) --- .../controller_manager_services.py | 31 ++++++++++++++++++- .../controller_manager/spawner.py | 1 + 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index bfe36fe3b7..36a924f101 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -55,6 +55,35 @@ class ServiceNotFoundError(Exception): pass +class SingletonServiceCaller: + """ + Singleton class to call services of controller manager. + + This class is used to create a service client for a given service name. + If the service client already exists, it returns the existing client. + It is used to avoid creating multiple service clients for the same service name. + + It needs Node object, service type and fully qualified service name to create a service client. + + """ + + _clients = {} + + def __new__(cls, node, service_type, fully_qualified_service_name): + if (node, fully_qualified_service_name) not in cls._clients: + cls._clients[(node, fully_qualified_service_name)] = node.create_client( + service_type, fully_qualified_service_name + ) + node.get_logger().debug( + f"{bcolors.MAGENTA}Creating a new service client : {fully_qualified_service_name} with node : {node.get_name()}{bcolors.ENDC}" + ) + + node.get_logger().debug( + f"{bcolors.OKBLUE}Returning the existing service client : {fully_qualified_service_name} for node : {node.get_name()}{bcolors.ENDC}" + ) + return cls._clients[(node, fully_qualified_service_name)] + + def service_caller( node, service_name, @@ -92,7 +121,7 @@ def service_caller( fully_qualified_service_name = ( f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name ) - cli = node.create_client(service_type, fully_qualified_service_name) + cli = SingletonServiceCaller(node, service_type, fully_qualified_service_name) while not cli.service_is_ready(): node.get_logger().info( diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index c5a23defe4..b8abe8469f 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -342,6 +342,7 @@ def main(args=None): node.get_logger().fatal(str(err)) return 1 finally: + node.destroy_node() rclpy.shutdown() From e7d2fcce4ea7fcca07d86c03cb53564a9aaecdba Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Wed, 18 Dec 2024 20:55:57 +0900 Subject: [PATCH 3/5] Make get_name() return a const reference (#1952) --- .../include/hardware_interface/loaned_command_interface.hpp | 2 +- .../include/hardware_interface/loaned_state_interface.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 6013dea778..7bb4d3a0ef 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -81,7 +81,7 @@ class LoanedCommandInterface } } - const std::string get_name() const { return command_interface_.get_name(); } + const std::string & get_name() const { return command_interface_.get_name(); } const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 3ebc8c7ca0..27b3da813e 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -75,7 +75,7 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + const std::string & get_name() const { return state_interface_.get_name(); } const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } From 65d40920b8a850dca7c05795210b22f92e31c675 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 19 Dec 2024 08:19:12 +0100 Subject: [PATCH 4/5] Add controller node options args to be able to set controller specific node arguments (#1713) --- controller_manager/CMakeLists.txt | 3 ++ .../controller_manager/spawner.py | 16 ++++++++ controller_manager/doc/userdoc.rst | 4 +- controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 35 +++++++++++++++++ .../test/test_controller/test_controller.cpp | 11 ++++++ .../test/test_controller/test_controller.hpp | 6 ++- .../test/test_spawner_unspawner.cpp | 39 +++++++++++++++++++ doc/release_notes.rst | 1 + .../hardware_interface/controller_info.hpp | 3 ++ 10 files changed, 116 insertions(+), 3 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1b0f308613..a97f171dd0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -58,10 +58,12 @@ target_link_libraries(ros2_control_node PRIVATE if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(example_interfaces REQUIRED) # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_link_libraries(test_controller PUBLIC controller_manager) + ament_target_dependencies(test_controller PUBLIC example_interfaces) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) install( @@ -210,6 +212,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_hardware_spawner controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index b8abe8469f..1a55f17e6d 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,6 +26,7 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters, set_controller_parameters_from_param_files, bcolors, ) @@ -145,6 +146,12 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--controller-ros-args", + help="The --ros-args to be passed to the controller node for remapping topics etc", + default=None, + required=False, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -203,6 +210,15 @@ def main(args=None): + bcolors.ENDC ) else: + if controller_ros_args := args.controller_ros_args: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "node_options_args", + controller_ros_args.split(), + ): + return 1 if param_files: if not set_controller_parameters_from_param_files( node, diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 4cbc7a73a1..f6967930eb 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -142,7 +142,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h 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] [--service-call-timeout SERVICE_CALL_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--controller-ros-args CONTROLLER_ROS_ARGS] controller_names [controller_names ...] positional arguments: @@ -167,6 +167,8 @@ There are two scripts to interact with controller manager from launch files: 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 + --controller-ros-args CONTROLLER_ROS_ARGS + The --ros-args to be passed to the controller node for remapping topics etc 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: diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5447040537..4bbd6306b1 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -36,6 +36,7 @@ python3-coverage hardware_interface_testing ros2_control_test_assets + example_interfaces ament_cmake diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f20e2b584..47ccf17cc8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -616,6 +616,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.fallback_controllers_names = fallback_controllers; } + const std::string node_options_args_param = controller_name + ".node_options_args"; + std::vector node_options_args; + if (!has_parameter(node_options_args_param)) + { + declare_parameter(node_options_args_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(node_options_args_param, node_options_args) && !node_options_args.empty()) + { + controller_spec.info.node_options_args = node_options_args; + } + return add_controller_impl(controller_spec); } @@ -3453,6 +3464,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } + // Add deprecation notice if the arguments are from the controller_manager node + if ( + check_for_element(node_options_arguments, RCL_REMAP_FLAG) || + check_for_element(node_options_arguments, RCL_SHORT_REMAP_FLAG)) + { + RCLCPP_WARN( + get_logger(), + "The use of remapping arguments to the controller_manager node is deprecated. Please use the " + "'--controller-ros-args' argument of the spawner to pass remapping arguments to the " + "controller node."); + } + for (const auto & parameters_file : controller.info.parameters_files) { if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) @@ -3475,6 +3498,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back("use_sim_time:=true"); } + // Add options parsed through the spawner + if ( + !controller.info.node_options_args.empty() && + !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG)) + { + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + } + for (const auto & arg : controller.info.node_options_args) + { + node_options_arguments.push_back(arg); + } + std::string arguments; arguments.reserve(1000); for (const auto & arg : node_options_arguments) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 04ae8c02c2..d20e7598a9 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -101,6 +101,17 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + const std::string service_name = get_node()->get_name() + std::string("/set_bool"); + service_ = get_node()->create_service( + service_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); + response->success = request->data; + }); return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d57fd9ddd9..ee9e668cfa 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,11 +15,14 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +#include #include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" +#include "example_interfaces/srv/set_bool.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace test_controller @@ -68,10 +71,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC std::vector get_state_interface_data() const; - const std::string & getRobotDescription() const; - void set_external_commands_for_testing(const std::vector & commands); + rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 107c557dbb..a4159d7fbe 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -664,6 +664,45 @@ TEST_F(TestLoadController, spawner_with_many_controllers) } } +TEST_F(TestLoadController, test_spawner_parsed_controller_ros_args) +{ + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + // Now as the controller is active, we can call check for the service + std::shared_ptr node = rclcpp::Node::make_shared("set_bool_client"); + auto set_bool_service = node->create_client("/set_bool"); + ASSERT_FALSE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_1_set_bool_service = + node->create_client("/ctrl_1/set_bool"); + ASSERT_TRUE(ctrl_1_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(ctrl_1_set_bool_service->service_is_ready()); + + // Now test the remapping of the service name with the controller_ros_args + EXPECT_EQ( + call_spawner( + "ctrl_2 -c test_controller_manager --controller-ros-args '-r /ctrl_2/set_bool:=/set_bool'"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + // Now as the controller is active, we can call check for the remapped service + ASSERT_TRUE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_2_set_bool_service = + node->create_client("/ctrl_2/set_bool"); + ASSERT_FALSE(ctrl_2_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(ctrl_2_set_bool_service->service_is_ready()); +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a187e62437..9b5360ced0 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 `_). +* The spawner now supports the ``--controller-ros-args`` argument to pass the ROS 2 node arguments to the controller node to be able to remap topics, services and etc (`#1713 `_). * The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). * ``--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 `_). diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 3ad89551d5..d814ca7ae2 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -41,6 +41,9 @@ struct ControllerInfo /// List of fallback controller names to be activated if this controller fails. std::vector fallback_controllers_names; + + /// Controller node options arguments + std::vector node_options_args; }; } // namespace hardware_interface From 612b30ba655922c51e7a07e077d3f3ca06ea2092 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 19 Dec 2024 09:45:00 +0100 Subject: [PATCH 5/5] Async Hardware Components (#1567) --- .../controller_manager/controller_manager.hpp | 9 - controller_manager/src/controller_manager.cpp | 5 - controller_manager/src/ros2_control_node.cpp | 2 - doc/release_notes.rst | 1 + hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/actuator_interface.hpp | 119 +++++++++++ .../hardware_interface/hardware_info.hpp | 2 + .../hardware_interface/resource_manager.hpp | 6 - .../hardware_interface/sensor_interface.hpp | 46 ++++ .../hardware_interface/system_interface.hpp | 119 +++++++++++ hardware_interface/package.xml | 1 + hardware_interface/src/actuator.cpp | 4 +- hardware_interface/src/component_parser.cpp | 32 +++ hardware_interface/src/resource_manager.cpp | 116 +--------- hardware_interface/src/sensor.cpp | 2 +- hardware_interface/src/system.cpp | 4 +- .../test/test_component_parser.cpp | 67 ++++++ .../test/test_resource_manager.cpp | 200 ++++++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 4 +- 19 files changed, 601 insertions(+), 139 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 332d565238..d33b167997 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -222,15 +222,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; - /// Deletes all async controllers and components. - /** - * Needed to join the threads immediately after the control loop is ended - * to avoid unnecessary iterations. Otherwise - * the threads will be joined only when the controller manager gets destroyed. - */ - CONTROLLER_MANAGER_PUBLIC - void shutdown_async_controllers_and_components(); - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 47ccf17cc8..61b9477e08 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2692,11 +2692,6 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } -void ControllerManager::shutdown_async_controllers_and_components() -{ - resource_manager_->shutdown_async_components(); -} - void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6bff653044..a8f8c079c3 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -147,8 +147,6 @@ int main(int argc, char ** argv) std::this_thread::sleep_until(next_iteration_time); } } - - cm->shutdown_async_controllers_and_components(); }); executor->add_node(cm); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9b5360ced0..69c282b051 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -157,6 +157,7 @@ hardware_interface * 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. +* With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. joint_limits ************ diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f9637b4f07..4e3fe6a9ae 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils rcutils + realtime_tools TinyXML2 tinyxml2_vendor joint_limits diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8aa214e728..6ca478231f 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -34,6 +34,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +112,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod clock_interface_ = clock_interface; actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -321,6 +346,50 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return return_type::OK; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read 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 + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -333,6 +402,49 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write 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 + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -426,6 +538,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::vector unlisted_commands_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; @@ -433,6 +546,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; + enum class TriggerType + { + READ, + WRITE + }; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index f62329ee62..9d96190954 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -178,6 +178,8 @@ struct HardwareInfo unsigned int rw_rate; /// Component is async bool is_async; + /// Async thread priority + int thread_priority; /// Name of the pluginlib plugin of the hardware that will be loaded. std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5592fc492..cdde7550d3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -426,12 +426,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); - /// Deletes all async components from the resource storage - /** - * Needed to join the threads immediately after the control loop is ended. - */ - void shutdown_async_components(); - /// Reads all loaded hardware components. /** * Reads from all active hardware components. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 455f6216a2..93de2a6494 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -34,6 +34,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +112,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + read_async_handler_ = std::make_unique>(); + read_async_handler_->init( + std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2), + info_.thread_priority); + read_async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -214,6 +225,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return state_interfaces; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * + * \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 + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while read async handler is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -294,6 +339,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector unlisted_states_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> read_async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 32d0b8a48a..ec649b0225 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -36,6 +36,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -114,6 +115,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -350,6 +375,50 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return return_type::OK; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read 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 + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -362,6 +431,49 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write 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 + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -453,6 +565,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. std::vector joint_states_; @@ -472,6 +585,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; + enum class TriggerType + { + READ, + WRITE + }; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 23b882ab02..c70ae6fc8b 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ pluginlib rclcpp_lifecycle rcpputils + realtime_tools tinyxml2_vendor joint_limits urdf diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 0e4ae95ca9..81bebf1182 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -302,7 +302,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); @@ -324,7 +324,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 146a5aef05..a3e9efaa3a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -60,6 +60,7 @@ constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; constexpr const auto kReadWriteRateAttribute = "rw_rate"; constexpr const auto kIsAsyncAttribute = "is_async"; +constexpr const auto kThreadPriorityAttribute = "thread_priority"; } // namespace @@ -273,6 +274,35 @@ bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) return attr ? parse_bool(attr->Value()) : false; } +/// Parse thread_priority attribute +/** + * Parses an XMLElement and returns the value of the thread_priority attribute. + * Defaults to 50 if not specified. + * + * \param[in] elem XMLElement that has the thread_priority attribute. + * \return positive integer specifying the thread priority. + */ +int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kThreadPriorityAttribute); + if (!attr) + { + return 50; + } + std::string s = attr->Value(); + std::regex int_re("[1-9][0-9]*"); + if (std::regex_match(s, int_re)) + { + return std::stoi(s); + } + else + { + throw std::runtime_error( + "Could not parse thread_priority tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + s + "\", but expected a non-zero positive integer."); + } +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -614,6 +644,8 @@ HardwareInfo parse_resource_from_xml( hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it); hardware.is_async = parse_is_async_attribute(ros2_control_it); + hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it) + : std::numeric_limits::max(); // Parse everything under ros2_control tag hardware.hardware_plugin_name = ""; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e77917af9d..0d9ad51eac 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -26,7 +26,6 @@ #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" @@ -302,15 +301,6 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } - - if (hardware_info_map_[hardware.get_name()].is_async) - { - async_component_threads_.emplace( - std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), - std::forward_as_tuple(cm_update_rate_, clock_interface_)); - - async_component_threads_.at(hardware.get_name()).register_component(&hardware); - } } if (!hardware.get_group_name().empty()) { @@ -431,7 +421,6 @@ class ResourceStorage if (result) { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); - async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); @@ -466,16 +455,6 @@ class ResourceStorage get_logger(), "Unknown exception occurred while activating hardware '%s'", hardware.get_name().c_str()); } - - if (result) - { - if (async_component_threads_.find(hardware.get_name()) != async_component_threads_.end()) - { - async_component_threads_.at(hardware.get_name()).activate(); - } - // TODO(destogl): make all command interfaces available (currently are all available) - } - return result; } @@ -827,15 +806,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_actuators(async_actuators_); - } - else - { - return load_and_init_actuators(actuators_); - } + return load_and_init_actuators(actuators_); } bool load_and_initialize_sensor(const HardwareInfo & hardware_info) @@ -860,14 +831,7 @@ class ResourceStorage return true; }; - if (hardware_info.is_async) - { - return load_and_init_sensors(async_sensors_); - } - else - { - return load_and_init_sensors(sensors_); - } + return load_and_init_sensors(sensors_); } bool load_and_initialize_system(const HardwareInfo & hardware_info) @@ -892,15 +856,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_systems(async_systems_); - } - else - { - return load_and_init_systems(systems_); - } + return load_and_init_systems(systems_); } void initialize_actuator( @@ -922,14 +878,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_actuators(async_actuators_); - } - else - { - init_actuators(actuators_); - } + init_actuators(actuators_); } void initialize_sensor( @@ -950,14 +899,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_sensors(async_sensors_); - } - else - { - init_sensors(sensors_); - } + init_sensors(sensors_); } void initialize_system( @@ -979,14 +921,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_systems(async_systems_); - } - else - { - init_systems(systems_); - } + init_systems(systems_); } void clear() @@ -995,10 +930,6 @@ class ResourceStorage sensors_.clear(); systems_.clear(); - async_actuators_.clear(); - async_sensors_.clear(); - async_systems_.clear(); - hardware_info_map_.clear(); state_interface_map_.clear(); command_interface_map_.clear(); @@ -1056,10 +987,6 @@ class ResourceStorage std::vector sensors_; std::vector systems_; - std::vector async_actuators_; - std::vector async_sensors_; - std::vector async_systems_; - std::unordered_map hardware_info_map_; std::unordered_map hw_group_state_; @@ -1083,9 +1010,6 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; - /// List of async components by type - std::unordered_map async_component_threads_; - // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1527,11 +1451,8 @@ std::unordered_map ResourceManager::get_comp }; loop_and_get_state(resource_storage_->actuators_); - loop_and_get_state(resource_storage_->async_actuators_); loop_and_get_state(resource_storage_->sensors_); - loop_and_get_state(resource_storage_->async_sensors_); loop_and_get_state(resource_storage_->systems_); - loop_and_get_state(resource_storage_->async_systems_); return resource_storage_->hardware_info_map_; } @@ -1799,35 +1720,10 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_actuators_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_systems_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_sensors_); - } return result; } -void ResourceManager::shutdown_async_components() -{ - resource_storage_->async_component_threads_.erase( - resource_storage_->async_component_threads_.begin(), - resource_storage_->async_component_threads_.end()); -} - // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ff193ff8e1..ae4d930bbe 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -256,7 +256,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 9a27aa4f68..89d6afd42e 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -300,7 +300,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); @@ -322,7 +322,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 8c535f04a9..05fe930e25 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -879,6 +879,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); ASSERT_THAT(hardware_info.joints, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); @@ -949,6 +951,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); @@ -1337,6 +1341,59 @@ TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limit EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) +{ + std::string urdf_to_test = ros2_control_test_assets::minimal_async_robot_urdf; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(3)); + auto hardware_info = control_hardware[0]; + + // Actuator + EXPECT_EQ(hardware_info.name, "TestActuatorHardware"); + EXPECT_EQ(hardware_info.type, "actuator"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 30); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + // Sensor + hardware_info = control_hardware[1]; + EXPECT_EQ(hardware_info.name, "TestSensorHardware"); + EXPECT_EQ(hardware_info.type, "sensor"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, IsEmpty()); + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 50); + + EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); + EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(1)); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty()); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); + + // System + hardware_info = control_hardware[2]; + EXPECT_EQ(hardware_info.name, "TestSystemHardware"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint2"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + EXPECT_EQ(hardware_info.joints[1].name, "joint3"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.gpios[0].name, "configuration"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 70); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -1377,6 +1434,16 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) } } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_invalid_thread_priority) +{ + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + "" + + std::string(ros2_control_test_assets::urdf_tail); + ; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5f7640546a..be9e672b3b 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1958,6 +1958,206 @@ TEST_F( check_read_and_write_cycles(false); } +class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + const auto minimal_robot_urdf_async = + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::async_hardware_resources) + + std::string(ros2_control_test_assets::urdf_tail); + rm = std::make_shared(node_, minimal_robot_urdf_async, false); + activate_components(*rm); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Check that all the components are async + ASSERT_TRUE(status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SYSTEM_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SENSOR_HARDWARE_NAME].is_async); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + // claimed_itfs[0].set_value(10.0); + // claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + void check_read_and_write_cycles(bool check_for_updated_values) + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + const double actuator_increment = 10.0; + const double system_increment = 20.0; + for (size_t i = 1; i < 100; i++) + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + // The values are computations exactly within the test_components + if (check_for_updated_values) + { + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + } + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + actuator_increment); + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + system_increment); + // This is needed to account for any missing hits to the read and write cycles as the tests + // are going to be run on a non-RT operating system + ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0); + ASSERT_NEAR(state_itfs[1].get_value(), prev_system_state_value, system_increment / 2.0); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + time = time + duration; + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); +}; + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_deactivate) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_unconfigured) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_finalized) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); 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 5f4512a9d1..1744a806bc 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 @@ -662,7 +662,7 @@ const auto hardware_resources = const auto async_hardware_resources = R"( - + test_actuator @@ -683,7 +683,7 @@ const auto async_hardware_resources = - + test_system 2