From 1a2b7680af3bd8979916f2980d51220b327da847 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Dec 2024 10:19:42 +0100 Subject: [PATCH] Fix the spawner to support full wildcard parameter entries (#1933) (cherry picked from commit bb087e2987ed4c64e34c9ae2cd410d2ac89a6827) # Conflicts: # .clang-format # .github/ISSUE_TEMPLATE/good-first-issue.md # .github/mergify.yml # .github/workflows/humble-abi-compatibility.yml # .github/workflows/humble-debian-build.yml # .github/workflows/prerelease-check.yml # .github/workflows/rolling-abi-compatibility.yml # .github/workflows/rolling-check-docs.yml # README.md # controller_interface/CHANGELOG.rst # controller_interface/CMakeLists.txt # controller_interface/include/controller_interface/chainable_controller_interface.hpp # controller_interface/include/controller_interface/controller_interface.hpp # controller_interface/include/controller_interface/controller_interface_base.hpp # controller_interface/include/controller_interface/helpers.hpp # controller_interface/include/semantic_components/range_sensor.hpp # controller_interface/package.xml # controller_interface/src/chainable_controller_interface.cpp # controller_interface/src/controller_interface.cpp # controller_interface/src/controller_interface_base.cpp # controller_interface/test/test_chainable_controller_interface.cpp # controller_interface/test/test_chainable_controller_interface.hpp # controller_interface/test/test_controller_interface.cpp # controller_interface/test/test_controller_interface.hpp # controller_interface/test/test_controller_with_options.cpp # controller_interface/test/test_controller_with_options.hpp # controller_interface/test/test_force_torque_sensor.hpp # controller_interface/test/test_imu_sensor.hpp # controller_interface/test/test_semantic_component_interface.hpp # controller_manager/CHANGELOG.rst # controller_manager/CMakeLists.txt # controller_manager/controller_manager/controller_manager_services.py # controller_manager/controller_manager/launch_utils.py # controller_manager/controller_manager/spawner.py # controller_manager/controller_manager/unspawner.py # controller_manager/doc/controller_chaining.rst # controller_manager/doc/images/chaining_example2.png # controller_manager/doc/userdoc.rst # controller_manager/include/controller_manager/controller_manager.hpp # controller_manager/include/controller_manager/controller_spec.hpp # controller_manager/package.xml # controller_manager/src/controller_manager.cpp # controller_manager/src/ros2_control_node.cpp # controller_manager/test/controller_manager_test_common.hpp # controller_manager/test/test_chainable_controller/test_chainable_controller.cpp # controller_manager/test/test_chainable_controller/test_chainable_controller.hpp # controller_manager/test/test_controller/test_controller.cpp # controller_manager/test/test_controller/test_controller.hpp # controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp # controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp # controller_manager/test/test_controller_manager.cpp # controller_manager/test/test_controller_manager_srvs.cpp # controller_manager/test/test_controller_manager_urdf_passing.cpp # controller_manager/test/test_controller_manager_with_namespace.cpp # controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp # controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp # controller_manager/test/test_controllers_chaining_with_controller_manager.cpp # controller_manager/test/test_hardware_management_srvs.cpp # controller_manager/test/test_hardware_spawner.cpp # controller_manager/test/test_load_controller.cpp # controller_manager/test/test_release_interfaces.cpp # controller_manager/test/test_spawner_unspawner.cpp # controller_manager_msgs/CHANGELOG.rst # controller_manager_msgs/CMakeLists.txt # controller_manager_msgs/msg/ControllerState.msg # controller_manager_msgs/msg/HardwareComponentState.msg # controller_manager_msgs/package.xml # controller_manager_msgs/srv/SwitchController.srv # doc/debugging.rst # doc/migration.rst # doc/release_notes.rst # hardware_interface/CHANGELOG.rst # hardware_interface/CMakeLists.txt # hardware_interface/doc/different_update_rates_userdoc.rst # hardware_interface/doc/hardware_interface_types_userdoc.rst # hardware_interface/doc/joints_userdoc.rst # hardware_interface/doc/mock_components_userdoc.rst # hardware_interface/doc/writing_new_hardware_component.rst # hardware_interface/include/hardware_interface/actuator.hpp # hardware_interface/include/hardware_interface/actuator_interface.hpp # hardware_interface/include/hardware_interface/component_parser.hpp # hardware_interface/include/hardware_interface/controller_info.hpp # hardware_interface/include/hardware_interface/handle.hpp # hardware_interface/include/hardware_interface/hardware_component_info.hpp # hardware_interface/include/hardware_interface/hardware_info.hpp # hardware_interface/include/hardware_interface/lexical_casts.hpp # hardware_interface/include/hardware_interface/loaned_command_interface.hpp # hardware_interface/include/hardware_interface/loaned_state_interface.hpp # hardware_interface/include/hardware_interface/resource_manager.hpp # hardware_interface/include/hardware_interface/sensor.hpp # hardware_interface/include/hardware_interface/sensor_interface.hpp # hardware_interface/include/hardware_interface/system.hpp # hardware_interface/include/hardware_interface/system_interface.hpp # hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp # hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp # hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp # hardware_interface/include/mock_components/generic_system.hpp # hardware_interface/package.xml # hardware_interface/src/actuator.cpp # hardware_interface/src/component_parser.cpp # hardware_interface/src/lexical_casts.cpp # hardware_interface/src/mock_components/generic_system.cpp # hardware_interface/src/resource_manager.cpp # hardware_interface/src/sensor.cpp # hardware_interface/src/system.cpp # hardware_interface/test/mock_components/test_generic_system.cpp # hardware_interface/test/test_component_interfaces.cpp # hardware_interface/test/test_component_parser.cpp # hardware_interface/test/test_handle.cpp # hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp # hardware_interface/test/test_hardware_components/test_hardware_components.xml # hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp # hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp # hardware_interface/test/test_hardware_components/test_two_joint_system.cpp # hardware_interface/test/test_inst_hardwares.cpp # hardware_interface/test/test_macros.cpp # hardware_interface_testing/CHANGELOG.rst # hardware_interface_testing/CMakeLists.txt # hardware_interface_testing/package.xml # hardware_interface_testing/test/test_components/test_actuator.cpp # hardware_interface_testing/test/test_components/test_components.xml # hardware_interface_testing/test/test_components/test_sensor.cpp # hardware_interface_testing/test/test_components/test_system.cpp # hardware_interface_testing/test/test_resource_manager.cpp # hardware_interface_testing/test/test_resource_manager.hpp # hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp # joint_limits/CHANGELOG.rst # joint_limits/CMakeLists.txt # joint_limits/include/joint_limits/joint_limits.hpp # joint_limits/include/joint_limits/joint_limits_rosparam.hpp # joint_limits/package.xml # joint_limits/test/joint_limits_rosparam.yaml # joint_limits/test/joint_limits_rosparam_test.cpp # ros2_control/CHANGELOG.rst # ros2_control/CMakeLists.txt # ros2_control/package.xml # ros2_control_test_assets/CHANGELOG.rst # ros2_control_test_assets/CMakeLists.txt # ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp # ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp # ros2_control_test_assets/package.xml # ros2controlcli/CHANGELOG.rst # ros2controlcli/doc/userdoc.rst # ros2controlcli/package.xml # ros2controlcli/ros2controlcli/api/__init__.py # ros2controlcli/ros2controlcli/verb/list_controller_types.py # ros2controlcli/ros2controlcli/verb/list_controllers.py # ros2controlcli/ros2controlcli/verb/list_hardware_components.py # ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py # ros2controlcli/ros2controlcli/verb/load_controller.py # ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py # ros2controlcli/ros2controlcli/verb/set_controller_state.py # ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py # ros2controlcli/ros2controlcli/verb/switch_controllers.py # ros2controlcli/ros2controlcli/verb/unload_controller.py # ros2controlcli/ros2controlcli/verb/view_controller_chains.py # ros2controlcli/setup.py # ros2controlcli/test/test_view_controller_chains.py # rqt_controller_manager/CHANGELOG.rst # rqt_controller_manager/package.xml # rqt_controller_manager/rqt_controller_manager/controller_manager.py # rqt_controller_manager/setup.py # transmission_interface/CHANGELOG.rst # transmission_interface/CMakeLists.txt # transmission_interface/include/transmission_interface/accessor.hpp # transmission_interface/include/transmission_interface/differential_transmission.hpp # transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp # transmission_interface/include/transmission_interface/handle.hpp # transmission_interface/include/transmission_interface/transmission.hpp # transmission_interface/package.xml # transmission_interface/src/differential_transmission_loader.cpp # transmission_interface/src/four_bar_linkage_transmission_loader.cpp # transmission_interface/src/simple_transmission_loader.cpp # transmission_interface/test/differential_transmission_loader_test.cpp # transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp # transmission_interface/test/four_bar_linkage_transmission_test.cpp # transmission_interface/test/simple_transmission_loader_test.cpp # transmission_interface/test/simple_transmission_test.cpp --- .clang-format | 4 + .github/ISSUE_TEMPLATE/good-first-issue.md | 10 + .github/mergify.yml | 52 + .github/pull_request_template.md | 16 + .../workflows/humble-abi-compatibility.yml | 4 + .github/workflows/humble-debian-build.yml | 4 + .github/workflows/prerelease-check.yml | 10 + .../workflows/rolling-abi-compatibility.yml | 4 + .github/workflows/rolling-check-docs.yml | 8 + MIGRATION.md | 12 + README.md | 4 + controller_interface/CHANGELOG.rst | 228 ++ controller_interface/CMakeLists.txt | 102 + .../chainable_controller_interface.hpp | 59 + .../controller_interface.hpp | 18 + .../controller_interface_base.hpp | 187 ++ .../include/controller_interface/helpers.hpp | 13 + .../semantic_components/range_sensor.hpp | 3 + controller_interface/package.xml | 17 + .../src/chainable_controller_interface.cpp | 192 ++ .../src/controller_interface.cpp | 16 + .../src/controller_interface_base.cpp | 152 ++ .../test_chainable_controller_interface.cpp | 132 ++ .../test_chainable_controller_interface.hpp | 67 + .../test/test_controller_interface.cpp | 86 + .../test/test_controller_interface.hpp | 3 + .../test/test_controller_node_options.yaml | 6 + .../test/test_controller_with_options.cpp | 115 + .../test/test_controller_with_options.hpp | 22 + .../test/test_force_torque_sensor.hpp | 18 + controller_interface/test/test_imu_sensor.hpp | 14 + .../test_semantic_component_interface.hpp | 8 + controller_manager/CHANGELOG.rst | 374 ++++ controller_manager/CMakeLists.txt | 254 +++ .../controller_manager_services.py | 169 ++ .../controller_manager/launch_utils.py | 18 + .../controller_manager/spawner.py | 111 + .../controller_manager/unspawner.py | 3 + .../doc/controller_chaining.rst | 66 + .../doc/parameters_context.yaml | 21 + controller_manager/doc/userdoc.rst | 215 ++ .../controller_manager/controller_manager.hpp | 216 ++ .../controller_manager/controller_spec.hpp | 37 + controller_manager/package.xml | 13 + controller_manager/src/controller_manager.cpp | 1921 ++++++++++++++++- .../src/controller_manager_parameters.yaml | 136 ++ controller_manager/src/ros2_control_node.cpp | 80 + .../test/controller_manager_test_common.hpp | 59 + .../test_chainable_controller.cpp | 85 + .../test_chainable_controller.hpp | 36 + .../test/test_controller/test_controller.cpp | 72 + .../test/test_controller/test_controller.hpp | 24 + .../test_controller_failed_init.cpp | 6 + .../test_controller_failed_init.hpp | 17 + .../test/test_controller_manager.cpp | 1570 ++++++++++++++ ...roller_manager_hardware_error_handling.cpp | 741 +++++++ .../test/test_controller_manager_srvs.cpp | 358 +++ .../test_controller_manager_urdf_passing.cpp | 197 ++ ...test_controller_manager_with_namespace.cpp | 12 + ...t_controller_spawner_wildcard_entries.yaml | 10 + ...ler_spawner_with_fallback_controllers.yaml | 13 + .../test_controller_with_interfaces.cpp | 3 + .../test_controller_with_interfaces.hpp | 5 + ...llers_chaining_with_controller_manager.cpp | 1595 ++++++++++++++ .../test/test_hardware_management_srvs.cpp | 82 + .../test/test_hardware_spawner.cpp | 4 + .../test/test_load_controller.cpp | 157 ++ .../test/test_release_interfaces.cpp | 62 + .../test/test_spawner_unspawner.cpp | 481 +++++ controller_manager_msgs/CHANGELOG.rst | 161 ++ controller_manager_msgs/CMakeLists.txt | 6 + .../msg/ControllerState.msg | 4 + .../msg/HardwareComponentState.msg | 5 + controller_manager_msgs/package.xml | 4 + .../srv/SwitchController.srv | 6 + doc/debugging.rst | 8 + doc/migration.rst | 167 ++ doc/release_notes.rst | 195 ++ hardware_interface/CHANGELOG.rst | 285 +++ hardware_interface/CMakeLists.txt | 108 + .../doc/different_update_rates_userdoc.rst | 86 + .../doc/hardware_interface_types_userdoc.rst | 115 + hardware_interface/doc/joints_userdoc.rst | 4 + .../doc/mock_components_userdoc.rst | 12 + .../doc/writing_new_hardware_component.rst | 132 ++ .../include/hardware_interface/actuator.hpp | 53 + .../hardware_interface/actuator_interface.hpp | 284 +++ .../hardware_interface/async_components.hpp | 121 ++ .../hardware_interface/component_parser.hpp | 40 + .../hardware_interface/controller_info.hpp | 6 + .../include/hardware_interface/handle.hpp | 209 ++ .../hardware_component_info.hpp | 21 + .../hardware_interface/hardware_info.hpp | 125 ++ .../hardware_interface/lexical_casts.hpp | 14 + .../hardware_interface/lifecycle_helpers.hpp | 30 + .../loaned_command_interface.hpp | 118 + .../loaned_state_interface.hpp | 94 + .../hardware_interface/resource_manager.hpp | 172 ++ .../include/hardware_interface/sensor.hpp | 51 + .../hardware_interface/sensor_interface.hpp | 200 ++ .../include/hardware_interface/system.hpp | 56 + .../hardware_interface/system_interface.hpp | 324 +++ .../hardware_interface_return_values.hpp | 4 + .../types/hardware_interface_type_values.hpp | 33 + .../types/lifecycle_state_names.hpp | 3 + .../mock_components/generic_system.hpp | 7 + hardware_interface/package.xml | 14 + hardware_interface/src/actuator.cpp | 269 +++ hardware_interface/src/component_parser.cpp | 538 +++++ hardware_interface/src/lexical_casts.cpp | 43 + .../src/mock_components/generic_system.cpp | 274 +++ hardware_interface/src/resource_manager.cpp | 1282 +++++++++++ hardware_interface/src/sensor.cpp | 220 ++ hardware_interface/src/system.cpp | 262 +++ .../mock_components/test_generic_system.cpp | 772 +++++++ .../test/test_component_interfaces.cpp | 1620 +++++++++++++- ...est_component_interfaces_custom_export.cpp | 385 ++++ .../test/test_component_parser.cpp | 1146 ++++++++++ hardware_interface/test/test_components.hpp | 39 + hardware_interface/test/test_handle.cpp | 39 + .../test_force_torque_sensor.cpp | 11 + .../test_hardware_components.xml | 9 + .../test_imu_sensor.cpp | 141 ++ .../test_single_joint_actuator.cpp | 23 + .../test_system_with_command_modes.cpp | 61 + .../test_two_joint_system.cpp | 32 + .../test/test_inst_hardwares.cpp | 5 + hardware_interface/test/test_macros.cpp | 8 + hardware_interface_testing/CHANGELOG.rst | 79 + hardware_interface_testing/CMakeLists.txt | 18 + hardware_interface_testing/package.xml | 4 + .../test/test_components/test_actuator.cpp | 89 + .../test/test_components/test_components.xml | 18 + .../test/test_components/test_sensor.cpp | 20 + .../test/test_components/test_system.cpp | 115 + .../test/test_resource_manager.cpp | 686 ++++++ .../test/test_resource_manager.hpp | 29 + ...esource_manager_prepare_perform_switch.cpp | 33 + joint_limits/CHANGELOG.rst | 183 ++ joint_limits/CMakeLists.txt | 102 + .../joint_limits/joint_limiter_interface.hpp | 249 +++ .../include/joint_limits/joint_limits.hpp | 21 + .../joint_limits/joint_limits_rosparam.hpp | 361 ++++ .../joint_limits/joint_limits_urdf.hpp | 85 + .../joint_limits/joint_saturation_limiter.hpp | 102 + .../include/joint_limits/visibility_control.h | 49 + joint_limits/joint_limiters.xml | 11 + joint_limits/package.xml | 24 + joint_limits/src/joint_limiter_interface.cpp | 21 + joint_limits/src/joint_saturation_limiter.cpp | 477 ++++ joint_limits/test/joint_limits_rosparam.yaml | 17 + .../test/joint_limits_rosparam_test.cpp | 56 + joint_limits/test/joint_limits_urdf_test.cpp | 175 ++ .../test/joint_saturation_limiter_param.yaml | 57 + .../test/test_joint_saturation_limiter.cpp | 569 +++++ .../test/test_joint_saturation_limiter.hpp | 106 + ros2_control/CHANGELOG.rst | 156 ++ ros2_control/CMakeLists.txt | 4 + ros2_control/package.xml | 4 + ros2_control_test_assets/CHANGELOG.rst | 184 ++ ros2_control_test_assets/CMakeLists.txt | 27 + .../components_urdfs.hpp | 385 ++++ .../ros2_control_test_assets/descriptions.hpp | 1755 ++++++++++++++- .../test_hardware_interface_constants.hpp | 28 + ros2_control_test_assets/package.xml | 4 + ros2controlcli/CHANGELOG.rst | 198 ++ ros2controlcli/doc/userdoc.rst | 198 ++ ros2controlcli/package.xml | 7 + ros2controlcli/ros2controlcli/api/__init__.py | 32 + .../verb/list_controller_types.py | 4 + .../ros2controlcli/verb/list_controllers.py | 36 + .../verb/list_hardware_components.py | 10 + .../verb/list_hardware_interfaces.py | 4 + .../ros2controlcli/verb/load_controller.py | 93 + .../verb/reload_controller_libraries.py | 4 + .../verb/set_controller_state.py | 10 + .../verb/set_hardware_component_state.py | 4 + .../ros2controlcli/verb/switch_controllers.py | 32 + .../ros2controlcli/verb/unload_controller.py | 18 + .../verb/view_controller_chains.py | 4 + ros2controlcli/setup.py | 4 + .../test/test_view_controller_chains.py | 4 + rqt_controller_manager/CHANGELOG.rst | 172 ++ rqt_controller_manager/package.xml | 4 + .../controller_manager.py | 14 + rqt_controller_manager/setup.py | 4 + transmission_interface/CHANGELOG.rst | 179 ++ transmission_interface/CMakeLists.txt | 85 + .../transmission_interface/accessor.hpp | 4 + .../differential_transmission.hpp | 3 + .../four_bar_linkage_transmission.hpp | 4 + .../include/transmission_interface/handle.hpp | 17 + .../transmission_interface/transmission.hpp | 3 + transmission_interface/package.xml | 8 + .../src/differential_transmission_loader.cpp | 6 + .../four_bar_linkage_transmission_loader.cpp | 6 + .../src/simple_transmission_loader.cpp | 6 + .../differential_transmission_loader_test.cpp | 30 + ...r_bar_linkage_transmission_loader_test.cpp | 30 + .../four_bar_linkage_transmission_test.cpp | 3 + .../test/simple_transmission_loader_test.cpp | 37 + .../test/simple_transmission_test.cpp | 3 + 202 files changed, 28725 insertions(+), 123 deletions(-) create mode 100644 .github/pull_request_template.md create mode 100644 MIGRATION.md create mode 100644 controller_interface/test/test_controller_node_options.yaml create mode 100644 controller_manager/doc/parameters_context.yaml create mode 100644 controller_manager/src/controller_manager_parameters.yaml create mode 100644 controller_manager/test/test_controller_manager_hardware_error_handling.cpp create mode 100644 controller_manager/test/test_controller_spawner_wildcard_entries.yaml create mode 100644 controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml create mode 100644 hardware_interface/include/hardware_interface/async_components.hpp create mode 100644 hardware_interface/include/hardware_interface/lifecycle_helpers.hpp create mode 100644 hardware_interface/test/test_component_interfaces_custom_export.cpp create mode 100644 hardware_interface/test/test_components.hpp create mode 100644 hardware_interface/test/test_hardware_components/test_imu_sensor.cpp create mode 100644 joint_limits/include/joint_limits/joint_limiter_interface.hpp create mode 100644 joint_limits/include/joint_limits/joint_limits_urdf.hpp create mode 100644 joint_limits/include/joint_limits/joint_saturation_limiter.hpp create mode 100644 joint_limits/include/joint_limits/visibility_control.h create mode 100644 joint_limits/joint_limiters.xml create mode 100644 joint_limits/src/joint_limiter_interface.cpp create mode 100644 joint_limits/src/joint_saturation_limiter.cpp create mode 100644 joint_limits/test/joint_limits_urdf_test.cpp create mode 100644 joint_limits/test/joint_saturation_limiter_param.yaml create mode 100644 joint_limits/test/test_joint_saturation_limiter.cpp create mode 100644 joint_limits/test/test_joint_saturation_limiter.hpp create mode 100644 ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp diff --git a/.clang-format b/.clang-format index 698bda7da8..e551d0ec5a 100644 --- a/.clang-format +++ b/.clang-format @@ -12,4 +12,8 @@ DerivePointerAlignment: false PointerAlignment: Middle ReflowComments: true IncludeBlocks: Preserve +<<<<<<< HEAD +======= +InsertBraces: true +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ... diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 8952a4f348..f3c65bb853 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,11 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! +<<<<<<< HEAD - [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/humble/Tutorials/Workspace/Creating-A-Workspace.html), for Step 3 use "Download Source Code" section with [these instructions](https://control.ros.org/humble/doc/getting_started/getting_started.html#building-from-source). +======= +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step 3 use "Download Source Code" section with [these instructions](https://control.ros.org/master/doc/getting_started/getting_started.html#building-from-source). +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_control`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_control` folder before cloning your own fork) @@ -53,8 +57,14 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! Furthermore, you find helpful resources here: +<<<<<<< HEAD * [ros2_control Contribution Guide](https://control.ros.org/humble/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/humble/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) +======= +* [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) +* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) +* [Robotics Stack Exchange](https://robotics.stackexchange.com) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) **Good luck with your first issue!** diff --git a/.github/mergify.yml b/.github/mergify.yml index 6bfd4470ac..27fd2c5f6c 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,4 +1,5 @@ pull_request_rules: +<<<<<<< HEAD - name: Backport to galactic at reviewers discretion conditions: - base=master @@ -16,11 +17,62 @@ pull_request_rules: backport: branches: - foxy +======= + - name: Backport to humble at reviewers discretion + conditions: + - base=master + - "label=backport-humble" + actions: + backport: + branches: + - humble + + - name: Backport to iron at reviewers discretion + conditions: + - base=master + - "label=backport-iron" + actions: + backport: + branches: + - iron +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) - name: Ask to resolve conflict conditions: - conflict +<<<<<<< HEAD - author!=mergify actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? +======= + - author!=mergify[bot] + - author!=dependabot[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: Ask to resolve conflict for backports + conditions: + - conflict + - author=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? + + - name: development targets master branch + conditions: + - base!=master + - author!=bmagyar + - author!=destogl + - author!=christophfroehlich + - author!=saikishor + - author!=mergify[bot] + - author!=dependabot[bot] + actions: + comment: + message: | + @{{author}}, all pull requests must be targeted towards the `master` development branch. + Once merged into `master`, it is possible to backport to `{{base}}`, but it must be in `master` + to have these changes reflected into new distributions. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000000..8e9178c167 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,16 @@ +Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: + +1. Limited scope. Your PR should do one thing or one set of things. Avoid adding “random fixes” to PRs. Put those on separate PRs. +2. Give your PR a descriptive title. Add a short summary, if required. +3. Make sure the pipeline is green. +4. Don’t be afraid to request reviews from maintainers. +5. New code = new tests. If you are adding new functionality, always make sure to add some tests exercising the code and serving as live documentation of your original intention. + +To send us a pull request, please: + +- [ ] Fork the repository. +- [ ] Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change. +- [ ] Ensure local tests pass. (`colcon test` and `pre-commit run` (requires you to install pre-commit by `pip3 install pre-commit`) +- [ ] Commit to your fork using clear commit messages. +- [ ] Send a pull request, answering any default questions in the pull request interface. +- [ ] Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation. diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 844a4559e2..2128711ae0 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -27,6 +27,10 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble +<<<<<<< HEAD ROS_REPO: main +======= + ROS_REPO: testing +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index 301befbcab..2150a340f6 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -34,4 +34,8 @@ jobs: upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master skip_packages: rqt_controller_manager +<<<<<<< HEAD skip_packages_test: controller_manager_msgs control_msgs +======= + skip_packages_test: controller_manager_msgs +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 809471897d..614f8ad38f 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -9,9 +9,14 @@ on: default: 'rolling' type: choice options: +<<<<<<< HEAD - foxy - galactic - humble +======= + - humble + - iron +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) - rolling branch: description: 'Chose branch for distro' @@ -19,9 +24,14 @@ on: default: 'master' type: choice options: +<<<<<<< HEAD - foxy - galactic - humble +======= + - humble + - iron +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) - master jobs: diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 3e30571ea1..a43836e3e6 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -27,6 +27,10 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: rolling +<<<<<<< HEAD ROS_REPO: main +======= + ROS_REPO: testing +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 8dc02f65c1..183eb572cc 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -9,6 +9,10 @@ on: - '**.rst' - '**.md' - '**.yaml' +<<<<<<< HEAD +======= + - '.github/workflows/rolling-check-docs.yml' +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) concurrency: group: ${{ github.workflow }}-${{ github.ref }} @@ -17,6 +21,10 @@ concurrency: jobs: check-docs: name: Check Docs +<<<<<<< HEAD uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master +======= + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) with: ROS2_CONTROL_PR: ${{ github.ref }} diff --git a/MIGRATION.md b/MIGRATION.md new file mode 100644 index 0000000000..f84fc7aa6f --- /dev/null +++ b/MIGRATION.md @@ -0,0 +1,12 @@ +# Migration Notes + +API changes in ros2_control releases + +## ROS Rolling + +#### Controller Interface + +`update_reference_from_subscribers()` method got parameters and now has the following signature: +``` +update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +``` diff --git a/README.md b/README.md index c7e74dd506..794b08e3a3 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,11 @@ # ros2_control [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +<<<<<<< HEAD [![codecov](https://codecov.io/gh/ros-controls/ros2_control/branch/humble/graph/badge.svg?token=idvm1zJXOf)](https://codecov.io/gh/ros-controls/ros2_control/tree/humble) +======= +[![codecov](https://codecov.io/gh/ros-controls/ros2_control/graph/badge.svg?token=idvm1zJXOf)](https://codecov.io/gh/ros-controls/ros2_control) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) This package is a part of the ros2_control framework. For more, please check the [documentation](https://control.ros.org/). diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c8ae3e5228..0dd179617f 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -135,6 +136,233 @@ Changelog for package controller_interface 2.15.0 (2022-09-19) ------------------- +======= +4.21.0 (2024-12-06) +------------------- +* [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (`#1871 `_) +* Contributors: Sai Kishor Kothakota + +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 + +4.19.0 (2024-10-26) +------------------- +* [CM] Async Function Handler for Controllers (`#1489 `_) +* Check the update_rate set to the controllers to be a valid one (`#1788 `_) +* [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_) +* Add `PoseSensor` semantic component (`#1775 `_) +* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) +* Contributors: RobertWilbrandt, Sai Kishor Kothakota + +4.18.0 (2024-10-07) +------------------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* [ControllerInterface] Fix to properly propagate the controller NodeOptions (`#1762 `_) +* [Controller Interface] Make assign and release interfaces virtual (`#1743 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + +4.17.0 (2024-09-11) +------------------- +* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) +* Contributors: Manuel Muth + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- +* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) +* Avoid using the global arguments for internal controller nodes (`#1694 `_) +* Contributors: Sai Kishor Kothakota + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* move critical variables to the private context (`#1623 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.13.0 (2024-07-08) +------------------- +* [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) +* Contributors: Sai Kishor Kothakota + +4.12.0 (2024-07-01) +------------------- + +4.11.0 (2024-05-14) +------------------- +* Fix dependencies for source build (`#1533 `_) +* Add find_package for ament_cmake_gen_version_h (`#1534 `_) +* Contributors: Christoph Fröhlich + +4.10.0 (2024-05-08) +------------------- +* Working async controllers and components [not synchronized] (`#1041 `_) +* Contributors: Márk Szitanics + +4.9.0 (2024-04-30) +------------------ +* return the proper const object of the pointer in the const method (`#1494 `_) +* Contributors: Sai Kishor Kothakota + +4.8.0 (2024-03-27) +------------------ +* generate version.h file per package using the ament_generate_version_header (`#1449 `_) +* Use ament_cmake generated rclcpp version header (`#1448 `_) +* Contributors: Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* add missing compiler definitions of RCLCPP_VERSION_MAJOR (`#1440 `_) +* Contributors: Sai Kishor Kothakota + +4.6.0 (2024-03-02) +------------------ +* Add -Werror=missing-braces to compile options (`#1423 `_) +* added conditioning to have rolling tags compilable in older versions (`#1422 `_) +* Contributors: Sai Kishor Kothakota + +4.5.0 (2024-02-12) +------------------ +* A method to get node options to setup the controller node #api-breaking (`#1169 `_) +* Contributors: Sai Kishor Kothakota + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ +* Issue 695: Changing 'namespace\_' variables to 'node_namespace' to make it more explicit (`#1239 `_) +* Contributors: bailaC + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* [ControllerInterface] Avoid warning about conversion from `int64_t` to `unsigned int` (`#1173 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* Pass URDF to controllers on init (`#1088 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- +* Enable services for setting the log-level in controller per default (`#1102 `_) +* Contributors: Dr. Denis + +3.18.0 (2023-08-17) +------------------- +* add a broadcaster for range sensor (`#1091 `_) +* Contributors: flochre + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- +* Add -Wconversion flag to protect future developments (`#1053 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#1037 `_) +* Contributors: Sai Kishor Kothakota, gwalck + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- +* Add missing build_export_depends to controller_interface (`#989 `_) +* Contributors: Scott K Logan + +3.12.0 (2023-04-02) +------------------- +* [Controller Interface] Add time and period paramters to update_reference_from_subscribers() (`#846 `_) #API-break +* Contributors: Robotgir, Denis Štogl + +3.11.0 (2023-03-22) +------------------- +* [ControllerManager] Add Class for Async Controllers and Lifecycle Management (`#932 `_) +* Contributors: Márk Szitanics + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Async params (`#927 `_) +* Contributors: Márk Szitanics, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ +* Update imu_sensor.hpp (`#893 `_) + Covariances values should come from the IMU_Broadcaster, like the frame_id or the time +* Contributors: flochre + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ +* Add docs in export interface configurations for controllers. (`#804 `_) +* Contributors: Denis Štogl + +3.0.0 (2022-09-19) +------------------ + +2.15.0 (2022-09-19) +------------------- +* Remove autodeclare of parameters for controllers. (`#757 `_) +* Contributors: Denis Štogl +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.14.0 (2022-09-04) ------------------- diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 0a380d5a13..0e9e7a3ca1 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(controller_interface) @@ -12,10 +13,34 @@ find_package(rclcpp_lifecycle REQUIRED) add_library( ${PROJECT_NAME} SHARED +======= +cmake_minimum_required(VERSION 3.16) +project(controller_interface LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Werror=missing-braces) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + rclcpp_lifecycle + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_gen_version_h REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(controller_interface SHARED +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) src/controller_interface_base.cpp src/controller_interface.cpp src/chainable_controller_interface.cpp ) +<<<<<<< HEAD target_include_directories( ${PROJECT_NAME} PRIVATE @@ -34,11 +59,84 @@ install(DIRECTORY include/ DESTINATION include ) install(TARGETS ${PROJECT_NAME} +======= +target_compile_features(controller_interface PUBLIC cxx_std_17) +target_include_directories(controller_interface PUBLIC + $ + $ +) +ament_target_dependencies(controller_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + + ament_add_gmock(test_controller_interface test/test_controller_interface.cpp) + target_link_libraries(test_controller_interface + controller_interface + ) + + ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp) + install(FILES test/test_controller_node_options.yaml + DESTINATION test) + target_link_libraries(test_controller_with_options + controller_interface + ) + + ament_add_gmock(test_chainable_controller_interface test/test_chainable_controller_interface.cpp) + target_link_libraries(test_chainable_controller_interface + controller_interface + hardware_interface::hardware_interface + ) + + ament_add_gmock(test_semantic_component_interface test/test_semantic_component_interface.cpp) + target_link_libraries(test_semantic_component_interface + controller_interface + hardware_interface::hardware_interface + ) + + ament_add_gmock(test_force_torque_sensor test/test_force_torque_sensor.cpp) + target_link_libraries(test_force_torque_sensor + controller_interface + hardware_interface::hardware_interface + ) + + ament_add_gmock(test_imu_sensor test/test_imu_sensor.cpp) + target_link_libraries(test_imu_sensor + controller_interface + hardware_interface::hardware_interface + ) + ament_target_dependencies(test_imu_sensor + sensor_msgs + ) + + ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp) + target_link_libraries(test_pose_sensor + controller_interface + hardware_interface::hardware_interface + ) + ament_target_dependencies(test_pose_sensor + geometry_msgs + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/controller_interface +) +install(TARGETS controller_interface + EXPORT export_controller_interface +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +<<<<<<< HEAD if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -109,5 +207,9 @@ ament_export_include_directories( ament_export_libraries( ${PROJECT_NAME} ) +======= +ament_export_targets(export_controller_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_package() ament_generate_version_header(${PROJECT_NAME}) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 8beafeeafe..476bdef8e5 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,12 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +<<<<<<< HEAD +======= +#include +#include +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "controller_interface/controller_interface_base.hpp" @@ -56,7 +62,14 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC +<<<<<<< HEAD std::vector export_reference_interfaces() final; +======= + std::vector export_state_interfaces() final; + + CONTROLLER_INTERFACE_PUBLIC + std::vector export_reference_interfaces() final; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -65,8 +78,24 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_in_chained_mode() const final; protected: +<<<<<<< HEAD /// Virtual method that each chainable controller should implement to export its chainable /// interfaces. +======= + /// Virtual method that each chainable controller should implement to export its read-only + /// chainable interfaces. + /** + * Each chainable controller implements this methods where all its state(read only) interfaces are + * exported. The method has the same meaning as `export_state_interfaces` method from + * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. + * + * \returns list of StateInterfaces that other controller can use as their inputs. + */ + virtual std::vector on_export_state_interfaces(); + + /// Virtual method that each chainable controller should implement to export its read/write + /// chainable interfaces. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Each chainable controller implements this methods where all input (command) interfaces are * exported. The method has the same meaning as `export_command_interface` method from @@ -74,7 +103,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \returns list of CommandInterfaces that other controller can use as their outputs. */ +<<<<<<< HEAD virtual std::vector on_export_reference_interfaces() = 0; +======= + virtual std::vector on_export_reference_interfaces(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Virtual method that each chainable controller should implement to switch chained mode. /** @@ -97,7 +130,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. */ +<<<<<<< HEAD virtual return_type update_reference_from_subscribers() = 0; +======= + virtual return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) = 0; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Execute calculations of the controller and update command interfaces. /** @@ -113,8 +151,29 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; +<<<<<<< HEAD + /// Storage of values for reference interfaces + std::vector reference_interfaces_; +======= + /// Storage of values for state interfaces + std::vector exported_state_interface_names_; + std::vector ordered_exported_state_interfaces_; + std::unordered_map + exported_state_interfaces_; + // BEGIN (Handle export change): for backward compatibility + std::vector state_interfaces_values_; + // END + /// Storage of values for reference interfaces + std::vector exported_reference_interface_names_; + // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + // END + std::vector + ordered_exported_reference_interfaces_; + std::unordered_map + exported_reference_interfaces_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..800d0ac02e 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -16,7 +16,10 @@ #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_ #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "controller_interface/controller_interface_base.hpp" @@ -43,12 +46,27 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase bool is_chainable() const final; /** +<<<<<<< HEAD +======= + * A non-chainable controller doesn't export any state interfaces. + * + * \returns empty list. + */ + CONTROLLER_INTERFACE_PUBLIC + std::vector export_state_interfaces() final; + + /** +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * Controller has no reference interfaces. * * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC +<<<<<<< HEAD std::vector export_reference_interfaces() final; +======= + std::vector export_reference_interfaces() final; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Controller is not chainable, therefore no chained mode can be set. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 972b31902b..2f37c373e4 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -17,15 +17,27 @@ #include #include +<<<<<<< HEAD #include #include "controller_interface/visibility_control.h" +======= +#include +#include + +#include "controller_interface/visibility_control.h" +#include "realtime_tools/async_function_handler.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/handle.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +<<<<<<< HEAD #include "rclcpp/rclcpp.hpp" +======= +#include "rclcpp/version.h" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace controller_interface @@ -58,6 +70,39 @@ struct InterfaceConfiguration std::vector names = {}; }; +<<<<<<< HEAD +======= +struct ControllerUpdateStats +{ + void reset() + { + total_triggers = 0; + failed_triggers = 0; + } + + unsigned int total_triggers; + unsigned int failed_triggers; +}; + +/** + * Struct to store the status of the controller update method. + * The status contains information if the update was triggered successfully, the result of the + * update method and the execution duration of the update method. The status is used to provide + * feedback to the controller_manager. + * @var successful: true if the update was triggered successfully, false if not. + * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. + * @var execution_time: duration of the execution of the update method. + * @var period: period of the update method. + */ +struct ControllerUpdateStatus +{ + bool successful = true; + return_type result = return_type::OK; + std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; +}; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Base interface class for an controller. The interface may not be used to implement a controller. * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface` @@ -97,13 +142,18 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * The configuration is used to check if controller can be activated and to claim interfaces from * hardware. * The claimed interfaces are populated in the +<<<<<<< HEAD * \ref ControllerInterfaceBase::state_interface_ "state_interface_" member. +======= + * \ref ControllerInterfaceBase::state_interfaces_ "state_interfaces_" member. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * * \returns configuration of state interfaces. */ CONTROLLER_INTERFACE_PUBLIC virtual InterfaceConfiguration state_interface_configuration() const = 0; +<<<<<<< HEAD CONTROLLER_INTERFACE_PUBLIC void assign_interfaces( std::vector && command_interfaces, @@ -119,6 +169,33 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)); +======= + /// Method that assigns the Loaned interfaces to the controller. + /** + * Method used by the controller_manager to assign the interfaces to the controller. + * \note When this method is overridden, the user has to also implement the `release_interfaces` + * method by overriding it to release the interfaces. + * + * \param[in] command_interfaces vector of command interfaces to be assigned to the controller. + * \param[in] state_interfaces vector of state interfaces to be assigned to the controller. + */ + CONTROLLER_INTERFACE_PUBLIC + virtual void assign_interfaces( + std::vector && command_interfaces, + std::vector && state_interfaces); + + /// Method that releases the Loaned interfaces from the controller. + /** + * Method used by the controller_manager to release the interfaces from the controller. + */ + CONTROLLER_INTERFACE_PUBLIC + virtual void release_interfaces(); + + CONTROLLER_INTERFACE_PUBLIC + return_type init( + const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, + const std::string & node_namespace, const rclcpp::NodeOptions & node_options); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Custom configure method to read additional parameters for controller-nodes /* @@ -137,24 +214,87 @@ 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 +<<<<<<< HEAD * \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 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. */ CONTROLLER_INTERFACE_PUBLIC virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; +<<<<<<< HEAD +======= + /** + * Trigger update method. This method is used by the controller_manager to trigger the update + * method of the controller. + * The method is used to trigger the update method of the controller synchronously or + * asynchronously, based on the controller configuration. + * **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 + * \returns ControllerUpdateStatus. The status contains information if the update was triggered + * successfully, the result of the update method and the execution duration of the update method. + */ + CONTROLLER_INTERFACE_PUBLIC + ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); CONTROLLER_INTERFACE_PUBLIC +<<<<<<< HEAD std::shared_ptr get_node() const; CONTROLLER_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_state() const; +======= + std::shared_ptr get_node() const; + + CONTROLLER_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & get_lifecycle_state() const; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; +<<<<<<< HEAD +======= + CONTROLLER_INTERFACE_PUBLIC + bool is_async() const; + + CONTROLLER_INTERFACE_PUBLIC + const std::string & get_robot_description() const; + + /** + * Method used by the controller_manager for base NodeOptions to instantiate the Lifecycle node + * of the controller upon loading the controller. + * + * \note The controller_manager will modify these NodeOptions in case a params file is passed + * by the spawner to load the controller parameters or when controllers are loaded in simulation + * (see ros2_control#1311, ros2_controllers#698 , ros2_controllers#795,ros2_controllers#966 for + * more details) + * + * @returns NodeOptions required for the configuration of the controller lifecycle node + */ + CONTROLLER_INTERFACE_PUBLIC + virtual rclcpp::NodeOptions define_custom_node_options() const + { + rclcpp::NodeOptions node_options; +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 21 + node_options.enable_logger_service(true); +#else + node_options.allow_undeclared_parameters(true); + node_options.automatically_declare_parameters_from_overrides(true); +#endif + return node_options; + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Declare and initialize a parameter with a type. /** * @@ -194,13 +334,33 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of command interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC +<<<<<<< HEAD virtual std::vector export_reference_interfaces() = 0; +======= + virtual std::vector + export_reference_interfaces() = 0; + + /** + * Export interfaces for a chainable controller that can be used as state interface by other + * controllers. + * + * \returns list of state interfaces for preceding controllers. + */ + CONTROLLER_INTERFACE_PUBLIC + virtual std::vector + export_state_interfaces() = 0; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode +<<<<<<< HEAD * usually involves disabling of subscribers and other external interfaces to avoid potential * concurrency in input commands. +======= + * usually involves the usage of the controller's reference interfaces by the other + * controllers +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * * \returns true if mode is switched successfully and false if not. */ @@ -218,6 +378,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; +<<<<<<< HEAD protected: std::vector command_interfaces_; std::vector state_interfaces_; @@ -225,6 +386,32 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; +======= + /** + * Method to wait for any running async update cycle to finish after finishing the current cycle. + * This is needed to be called before deactivating the controller by the controller_manager, so + * that the interfaces still exist when the controller finishes its cycle and then it's exits. + * + * \note **The method is not real-time safe and shouldn't be called in the control loop.** + * + * If the controller is running in async mode, the method will wait for the current async update + * to finish. If the controller is not running in async mode, the method will do nothing. + */ + CONTROLLER_INTERFACE_PUBLIC + void wait_for_trigger_update_to_finish(); + +protected: + std::vector command_interfaces_; + std::vector state_interfaces_; + +private: + std::shared_ptr node_; + std::unique_ptr> async_handler_; + unsigned int update_rate_ = 0; + bool is_async_ = false; + std::string urdf_ = ""; + ControllerUpdateStats trigger_stats_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index b571751f55..032c7ca305 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -78,6 +78,19 @@ inline bool interface_list_contains_interface_type( interface_type_list.end(); } +<<<<<<< HEAD +======= +template +void add_element_to_list(std::vector & list, const T & element) +{ + if (std::find(list.begin(), list.end(), element) == list.end()) + { + // Only add to the list if it doesn't exist + list.push_back(element); + } +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace controller_interface #endif // CONTROLLER_INTERFACE__HELPERS_HPP_ diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index baa9022c75..82374ec7e4 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -15,7 +15,10 @@ #ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 2edf44e015..186bba6831 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,11 @@ controller_interface +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Description of controller_interface Bence Magyar Denis Štogl @@ -12,6 +16,7 @@ ament_cmake_gen_version_h hardware_interface +<<<<<<< HEAD rclcpp_lifecycle sensor_msgs @@ -22,6 +27,18 @@ hardware_interface rclcpp_lifecycle +======= + realtime_tools + rclcpp_lifecycle + sensor_msgs + + hardware_interface + realtime_tools + rclcpp_lifecycle + + realtime_tools + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_cmake_gmock geometry_msgs sensor_msgs diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 45e2f1a32e..c654f65fe0 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -16,6 +16,10 @@ #include +<<<<<<< HEAD +======= +#include "controller_interface/helpers.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -32,7 +36,11 @@ return_type ChainableControllerInterface::update( if (!is_in_chained_mode()) { +<<<<<<< HEAD ret = update_reference_from_subscribers(); +======= + ret = update_reference_from_subscribers(time, period); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (ret != return_type::OK) { return ret; @@ -44,6 +52,7 @@ return_type ChainableControllerInterface::update( return ret; } +<<<<<<< HEAD std::vector ChainableControllerInterface::export_reference_interfaces() { @@ -82,13 +91,162 @@ ChainableControllerInterface::export_reference_interfaces() } return reference_interfaces; +======= +std::vector +ChainableControllerInterface::export_state_interfaces() +{ + auto state_interfaces = on_export_state_interfaces(); + std::vector state_interfaces_ptrs_vec; + state_interfaces_ptrs_vec.reserve(state_interfaces.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces.size()); + exported_state_interface_names_.reserve(state_interfaces.size()); + + // check if the names of the controller state interfaces begin with the controller's name + for (const auto & interface : state_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + std::string error_msg = + "The prefix of the interface '" + interface.get_prefix_name() + + "' does not equal the controller's name '" + get_node()->get_name() + + "'. This is mandatory for state interfaces. No state interface will be exported. Please " + "correct and recompile the controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + auto state_interface = std::make_shared(interface); + const auto interface_name = state_interface->get_interface_name(); + auto [it, succ] = exported_state_interfaces_.insert({interface_name, state_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert StateInterface<" + interface_name + + "> into exported_state_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + exported_state_interfaces_.clear(); + exported_state_interface_names_.clear(); + state_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + ordered_exported_state_interfaces_.push_back(state_interface); + add_element_to_list(exported_state_interface_names_, interface_name); + state_interfaces_ptrs_vec.push_back( + std::const_pointer_cast(state_interface)); + } + + if (exported_state_interfaces_.size() != state_interfaces.size()) + { + std::string error_msg = + "The internal storage for state interface ptrs 'exported_state_interfaces_' variable has " + "size '" + + std::to_string(exported_state_interfaces_.size()) + + "', but it is expected to have the size '" + std::to_string(state_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + + return state_interfaces_ptrs_vec; +} + +std::vector +ChainableControllerInterface::export_reference_interfaces() +{ + auto reference_interfaces = on_export_reference_interfaces(); + std::vector reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + exported_reference_interface_names_.reserve(reference_interfaces.size()); + ordered_exported_reference_interfaces_.reserve(reference_interfaces.size()); + + // BEGIN (Handle export change): for backward compatibility + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + // END + + // check if the names of the reference interfaces begin with the controller's name + const auto ref_interface_size = reference_interfaces.size(); + for (auto & interface : reference_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + std::string error_msg = "The name of the interface " + interface.get_name() + + " does not begin with the controller's name. This is mandatory for " + "reference interfaces. Please " + "correct and recompile the controller with name " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); + } + + hardware_interface::CommandInterface::SharedPtr reference_interface = + std::make_shared(std::move(interface)); + const auto interface_name = reference_interface->get_interface_name(); + // check the exported interface name is unique + auto [it, succ] = exported_reference_interfaces_.insert({interface_name, reference_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert Reference interface<" + interface_name + + "> into reference_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + reference_interfaces_.clear(); + exported_reference_interface_names_.clear(); + reference_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + ordered_exported_reference_interfaces_.push_back(reference_interface); + add_element_to_list(exported_reference_interface_names_, interface_name); + reference_interfaces_ptrs_vec.push_back(reference_interface); + } + + if (exported_reference_interfaces_.size() != ref_interface_size) + { + std::string error_msg = + "The internal storage for exported reference ptrs 'exported_reference_interfaces_' variable " + "has size '" + + std::to_string(exported_reference_interfaces_.size()) + + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + + return reference_interfaces_ptrs_vec; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) { bool result = false; +<<<<<<< HEAD if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { result = on_set_chained_mode(chained_mode); @@ -103,7 +261,12 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) get_node()->get_logger(), "Can not change controller's chained mode because it is no in '%s' state. " "Current state is '%s'.", +<<<<<<< HEAD hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); +======= + hardware_interface::lifecycle_state_names::UNCONFIGURED, + get_lifecycle_state().label().c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; @@ -113,4 +276,33 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } +<<<<<<< HEAD +======= +std::vector +ChainableControllerInterface::on_export_state_interfaces() +{ + state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); + std::vector state_interfaces; + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + { + state_interfaces.emplace_back( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]); + } + return state_interfaces; +} + +std::vector +ChainableControllerInterface::on_export_reference_interfaces() +{ + reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0); + std::vector reference_interfaces; + for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i) + { + reference_interfaces.emplace_back(hardware_interface::CommandInterface( + get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i])); + } + return reference_interfaces; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace controller_interface diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 392a48ffa4..92600e44b8 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -14,6 +14,7 @@ #include "controller_interface/controller_interface.hpp" +<<<<<<< HEAD #include #include #include @@ -22,13 +23,28 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace controller_interface { ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } +<<<<<<< HEAD std::vector ControllerInterface::export_reference_interfaces() +======= +std::vector +ControllerInterface::export_state_interfaces() +{ + return {}; +} + +std::vector +ControllerInterface::export_reference_interfaces() +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return {}; } diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 26c8c41cf6..a25e98a600 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -16,15 +16,21 @@ #include #include +<<<<<<< HEAD #include #include #include "hardware_interface/types/lifecycle_state_names.hpp" +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "lifecycle_msgs/msg/state.hpp" namespace controller_interface { return_type ControllerInterfaceBase::init( +<<<<<<< HEAD const std::string & controller_name, const std::string & namespace_, const rclcpp::NodeOptions & node_options) { @@ -34,6 +40,24 @@ return_type ControllerInterfaceBase::init( try { auto_declare("update_rate", 0); +======= + const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, + const std::string & node_namespace, const rclcpp::NodeOptions & node_options) +{ + urdf_ = urdf; + update_rate_ = cm_update_rate; + node_ = std::make_shared( + controller_name, node_namespace, node_options, + false); // disable LifecycleNode service interfaces + + try + { + // no rclcpp::ParameterValue unsigned int specialization + auto_declare("update_rate", static_cast(update_rate_)); + + auto_declare("is_async", false); + auto_declare("thread_priority", 50); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } catch (const std::exception & e) { @@ -54,10 +78,32 @@ return_type ControllerInterfaceBase::init( std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1)); node_->register_on_cleanup( +<<<<<<< HEAD std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); 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()) + { + async_handler_->stop_thread(); + } + return on_cleanup(previous_state); + }); + + node_->register_on_activate( + [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); + }); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) node_->register_on_deactivate( std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1)); @@ -81,10 +127,50 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Then we don't need to do state-machine related checks. // // Other solution is to add check into the LifecycleNode if a transition is valid to trigger +<<<<<<< HEAD if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); } +======= + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + const auto update_rate = get_node()->get_parameter("update_rate").as_int(); + if (update_rate < 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!"); + return get_lifecycle_state(); + } + if (update_rate_ != 0u && update_rate > update_rate_) + { + RCLCPP_WARN( + get_node()->get_logger(), + "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the " + "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.", + update_rate, update_rate_); + } + else + { + update_rate_ = static_cast(update_rate); + } + is_async_ = get_node()->get_parameter("is_async").as_bool(); + } + if (is_async_) + { + const int thread_priority = + static_cast(get_node()->get_parameter("thread_priority").as_int()); + RCLCPP_INFO( + get_node()->get_logger(), "Starting async handler with scheduler priority: %d", + thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + std::bind( + &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2), + thread_priority); + async_handler_->start_thread(); + } + trigger_stats_.reset(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return get_node()->configure(); } @@ -103,11 +189,59 @@ void ControllerInterfaceBase::release_interfaces() state_interfaces_.clear(); } +<<<<<<< HEAD const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const +======= +const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return node_->get_current_state(); } +<<<<<<< HEAD +======= +ControllerUpdateStatus ControllerInterfaceBase::trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + ControllerUpdateStatus status; + trigger_stats_.total_triggers++; + if (is_async()) + { + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); + const auto result = async_handler_->trigger_async_callback(time, period); + if (!result.first) + { + trigger_stats_.failed_triggers++; + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 20000, + "The controller missed %u update cycles out of %u total triggers.", + trigger_stats_.failed_triggers, trigger_stats_.total_triggers); + } + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } + } + else + { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = update(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; + } + return status; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::shared_ptr ControllerInterfaceBase::get_node() { if (!node_.get()) @@ -117,7 +251,11 @@ std::shared_ptr ControllerInterfaceBase::get_no return node_; } +<<<<<<< HEAD std::shared_ptr ControllerInterfaceBase::get_node() const +======= +std::shared_ptr ControllerInterfaceBase::get_node() const +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { if (!node_.get()) { @@ -128,4 +266,18 @@ std::shared_ptr ControllerInterfaceBase::get_no unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; } +<<<<<<< HEAD +======= +bool ControllerInterfaceBase::is_async() const { return is_async_; } + +const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } + +void ControllerInterfaceBase::wait_for_trigger_update_to_finish() +{ + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->wait_for_trigger_cycle_to_finish(); + } +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 6ae36b39ce..a85dc6a53b 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -15,29 +15,74 @@ #include "test_chainable_controller_interface.hpp" #include +<<<<<<< HEAD +======= +#include + +using ::testing::IsEmpty; +using ::testing::SizeIs; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(ChainableControllerInterfaceTest, default_returns) { TestableChainableControllerInterface controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_NO_THROW(controller.get_node()); EXPECT_TRUE(controller.is_chainable()); EXPECT_FALSE(controller.is_in_chained_mode()); } +<<<<<<< HEAD +======= +TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + auto exported_state_interfaces = controller.export_state_interfaces(); + + ASSERT_THAT(exported_state_interfaces, SizeIs(1)); + EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); + + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) { TestableChainableControllerInterface controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); +<<<<<<< HEAD ASSERT_EQ(reference_interfaces.size(), 1u); EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); @@ -46,10 +91,21 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) } TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) +======= + ASSERT_THAT(reference_interfaces, SizeIs(1)); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); + + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); +} + +TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { TestableChainableControllerInterface controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); @@ -65,13 +121,32 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node // initialize, create node ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_NO_THROW(controller.get_node()); controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); // expect empty return because interface prefix is not equal to the node name +<<<<<<< HEAD auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_TRUE(reference_interfaces.empty()); +======= + std::vector exported_reference_interfaces; + EXPECT_THROW( + { exported_reference_interfaces = controller.export_reference_interfaces(); }, + std::runtime_error); + ASSERT_THAT(exported_reference_interfaces, IsEmpty()); + // expect empty return because interface prefix is not equal to the node name + std::vector exported_state_interfaces; + EXPECT_THROW( + { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error); + ASSERT_THAT(exported_state_interfaces, IsEmpty()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) @@ -79,16 +154,33 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TestableChainableControllerInterface controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + auto reference_interfaces = controller.export_reference_interfaces(); + ASSERT_THAT(reference_interfaces, SizeIs(1)); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, SizeIs(1)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode +<<<<<<< HEAD EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); +======= + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -97,10 +189,18 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Success setting chained mode +<<<<<<< HEAD reference_interfaces[0].set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); +======= + reference_interfaces[0]->set_value(0.0); + + EXPECT_TRUE(controller.set_chained_mode(true)); + EXPECT_TRUE(controller.is_in_chained_mode()); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -126,9 +226,19 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) TestableChainableControllerInterface controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + EXPECT_FALSE(controller.set_chained_mode(false)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(controller.is_in_chained_mode()); // call update and update it from subscriber because not in chained mode @@ -136,6 +246,10 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); +<<<<<<< HEAD +======= + ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -143,6 +257,10 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); +<<<<<<< HEAD +======= + ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Provoke error from update - return ERROR, but reference interface is updated and not reduced controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); @@ -150,6 +268,10 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); +<<<<<<< HEAD +======= + ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller.reference_interfaces_[0] = 0.0; @@ -163,6 +285,11 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_EQ(controller.reference_interfaces_[0], -1.0); +<<<<<<< HEAD +======= + ASSERT_EQ( + controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Provoke error from update - return ERROR, but reference interface is updated directly controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); @@ -171,4 +298,9 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); +<<<<<<< HEAD +======= + ASSERT_EQ( + controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 38ce124582..996352a156 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -15,6 +15,7 @@ #ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ +<<<<<<< HEAD #include #include @@ -23,30 +24,57 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +======= +#include + +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller"; constexpr double INTERFACE_VALUE = 1989.0; constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; +<<<<<<< HEAD +======= +constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; +constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface { public: +<<<<<<< HEAD FRIEND_TEST(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size); +======= + FRIEND_TEST(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic); TestableChainableControllerInterface() { reference_interfaces_.reserve(1); reference_interfaces_.push_back(INTERFACE_VALUE); +<<<<<<< HEAD +======= + state_interfaces_values_.reserve(1); + state_interfaces_values_.push_back(EXPORTED_STATE_INTERFACE_VALUE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } controller_interface::CallbackReturn on_init() override { // set default value +<<<<<<< HEAD name_prefix_of_reference_interfaces_ = get_node()->get_name(); +======= + name_prefix_of_interfaces_ = get_node()->get_name(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_interface::CallbackReturn::SUCCESS; } @@ -64,12 +92,30 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods +<<<<<<< HEAD +======= + std::vector on_export_state_interfaces() override + { + std::vector state_interfaces; + + state_interfaces.push_back(hardware_interface::StateInterface( + name_prefix_of_interfaces_, "test_state", &state_interfaces_values_[0])); + + return state_interfaces; + } + + // Implementation of ChainableController virtual methods +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::vector on_export_reference_interfaces() override { std::vector command_interfaces; command_interfaces.push_back(hardware_interface::CommandInterface( +<<<<<<< HEAD name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0])); +======= + name_prefix_of_interfaces_, "test_itf", &reference_interfaces_[0])); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return command_interfaces; } @@ -78,6 +124,10 @@ class TestableChainableControllerInterface { if (reference_interfaces_[0] == 0.0) { +<<<<<<< HEAD +======= + state_interfaces_values_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return true; } else @@ -86,7 +136,12 @@ class TestableChainableControllerInterface } } +<<<<<<< HEAD controller_interface::return_type update_reference_from_subscribers() override +======= + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { if (reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR) { @@ -106,13 +161,21 @@ class TestableChainableControllerInterface } reference_interfaces_[0] -= 1; +<<<<<<< HEAD +======= + state_interfaces_values_[0] += 1; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_interface::return_type::OK; } void set_name_prefix_of_reference_interfaces(const std::string & prefix) { +<<<<<<< HEAD name_prefix_of_reference_interfaces_ = prefix; +======= + name_prefix_of_interfaces_ = prefix; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void set_new_reference_interface_value(const double ref_itf_value) @@ -120,7 +183,11 @@ class TestableChainableControllerInterface reference_interface_value_ = ref_itf_value; } +<<<<<<< HEAD std::string name_prefix_of_reference_interfaces_; +======= + std::string name_prefix_of_interfaces_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF; }; diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 567efe52ff..0cb01f3897 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -16,7 +16,14 @@ #include #include +<<<<<<< HEAD +======= +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -38,6 +45,7 @@ TEST(TestableControllerInterface, init) ASSERT_THROW(controller.get_node(), std::runtime_error); // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); @@ -47,10 +55,25 @@ TEST(TestableControllerInterface, init) // Even after configure is 0 controller.configure(); ASSERT_EQ(controller.get_update_rate(), 0u); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options), + controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 10u); + + // Even after configure is 10 + controller.configure(); + ASSERT_EQ(controller.get_update_rate(), 10u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclcpp::shutdown(); } +<<<<<<< HEAD TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file @@ -61,6 +84,50 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) TestableControllerInterface controller; // initialize, create node ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); +======= +TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + rclcpp::init(0, nullptr); + + TestableControllerInterface controller; + // initialize, create node + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=-100"); + node_options = node_options.arguments(node_options_arguments); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options), + controller_interface::return_type::OK); + + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 1000u); + + // The configure should fail and the update rate should stay the same + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(controller.get_update_rate(), 1000u); + rclcpp::shutdown(); +} + +TEST(TestableControllerInterface, setting_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + rclcpp::init(0, nullptr); + + TestableControllerInterface controller; + // initialize, create node + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=2812"); + node_options = node_options.arguments(node_options_arguments); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options), + controller_interface::return_type::OK); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // initialize executor to be able to get parameter update auto executor = @@ -74,8 +141,13 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) using namespace std::chrono_literals; std::this_thread::sleep_for(50ms); +<<<<<<< HEAD // update_rate is set to default 0 because it is set on configure ASSERT_EQ(controller.get_update_rate(), 0u); +======= + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 5000u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Even after configure is 0 controller.configure(); @@ -123,7 +195,14 @@ TEST(TestableControllerInterfaceInitError, init_with_error) TestableControllerInterfaceInitError controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options), + controller_interface::return_type::ERROR); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclcpp::shutdown(); } @@ -137,7 +216,14 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) TestableControllerInterfaceInitFailure controller; // initialize, create node +<<<<<<< HEAD ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR); +======= + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::ERROR); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclcpp::shutdown(); } diff --git a/controller_interface/test/test_controller_interface.hpp b/controller_interface/test/test_controller_interface.hpp index 426890220a..4e69d455ec 100644 --- a/controller_interface/test/test_controller_interface.hpp +++ b/controller_interface/test/test_controller_interface.hpp @@ -16,7 +16,10 @@ #define TEST_CONTROLLER_INTERFACE_HPP_ #include "controller_interface/controller_interface.hpp" +<<<<<<< HEAD #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface"; diff --git a/controller_interface/test/test_controller_node_options.yaml b/controller_interface/test/test_controller_node_options.yaml new file mode 100644 index 0000000000..0c23c7f9d8 --- /dev/null +++ b/controller_interface/test/test_controller_node_options.yaml @@ -0,0 +1,6 @@ +controller_name: + ros__parameters: + parameter_list: + parameter1: 20.0 + parameter2: 23.14 + parameter3: -52.323 diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 080b8c4f8d..25d3ccb746 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -16,8 +16,12 @@ #include #include +<<<<<<< HEAD #include "rclcpp/rclcpp.hpp" +======= +#include "ament_index_cpp/get_package_prefix.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions { @@ -42,11 +46,21 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; +<<<<<<< HEAD EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK); +======= + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), + controller_interface::return_type::OK); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); +<<<<<<< HEAD +======= + EXPECT_TRUE(node_options.arguments().empty()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // checks that the parameters have been correctly processed EXPECT_EQ(controller.params.size(), 3u); EXPECT_EQ(controller.params["parameter1"], 1.); @@ -55,6 +69,101 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::shutdown(); } +<<<<<<< HEAD +======= +TEST(ControllerWithOption, init_with_node_options_arguments_parameters) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments( + {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.", + "-p", "parameter_list.parameter3:=3."}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(7lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 1.); + EXPECT_EQ(controller.params["parameter2"], 2.); + EXPECT_EQ(controller.params["parameter3"], 3.); + rclcpp::shutdown(); +} + +TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") + + "/test/test_controller_node_options.yaml"; + std::cerr << params_file_path << std::endl; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments({"--ros-args", "--params-file", params_file_path}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(3lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 20.0); + EXPECT_EQ(controller.params["parameter2"], 23.14); + EXPECT_EQ(controller.params["parameter3"], -52.323); + bool use_sim_time(true); + controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); + ASSERT_FALSE(use_sim_time); + rclcpp::shutdown(); +} + +TEST( + ControllerWithOption, init_with_node_options_arguments_parameters_file_and_override_command_line) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") + + "/test/test_controller_node_options.yaml"; + std::cerr << params_file_path << std::endl; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments( + {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785", + "-p", "use_sim_time:=true"}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(7lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 562.785); + EXPECT_EQ(controller.params["parameter2"], 23.14); + EXPECT_EQ(controller.params["parameter3"], -52.323); + bool use_sim_time(false); + controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); + ASSERT_TRUE(use_sim_time); + rclcpp::shutdown(); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST(ControllerWithOption, init_without_overrides) { // mocks the declaration of overrides parameters in a yaml file @@ -63,7 +172,13 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; +<<<<<<< HEAD EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR); +======= + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), + controller_interface::return_type::ERROR); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index e19fc18d43..c790a2324c 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -16,11 +16,17 @@ #define TEST_CONTROLLER_WITH_OPTIONS_HPP_ #include +<<<<<<< HEAD #include #include #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" +======= +#include + +#include "controller_interface/controller_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace controller_with_options { @@ -36,6 +42,7 @@ class ControllerWithOptions : public controller_interface::ControllerInterface ControllerWithOptions() = default; LifecycleNodeInterface::CallbackReturn on_init() override { +<<<<<<< HEAD return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -65,6 +72,21 @@ class ControllerWithOptions : public controller_interface::ControllerInterface { return controller_interface::return_type::ERROR; } +======= + if (get_node()->get_parameters("parameter_list", params)) + { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "I found " << params.size() << " parameters."); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + return LifecycleNodeInterface::CallbackReturn::FAILURE; + } + + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } controller_interface::InterfaceConfiguration command_interface_configuration() const override diff --git a/controller_interface/test/test_force_torque_sensor.hpp b/controller_interface/test/test_force_torque_sensor.hpp index 2d61f665c3..7aa4fb1859 100644 --- a/controller_interface/test/test_force_torque_sensor.hpp +++ b/controller_interface/test/test_force_torque_sensor.hpp @@ -19,12 +19,20 @@ #ifndef TEST_FORCE_TORQUE_SENSOR_HPP_ #define TEST_FORCE_TORQUE_SENSOR_HPP_ +<<<<<<< HEAD +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include +<<<<<<< HEAD #include "gmock/gmock.h" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "semantic_components/force_torque_sensor.hpp" // implementing and friending so we can access member variables @@ -68,6 +76,7 @@ class ForceTorqueSensorTest : public ::testing::Test protected: const size_t size_ = 6; const std::string sensor_name_ = "test_FTS"; +<<<<<<< HEAD std::array force_values_ = {1.1, 2.2, 3.3}; std::array torque_values_ = {4.4, 5.5, 6.6}; std::unique_ptr force_torque_sensor_; @@ -75,6 +84,15 @@ class ForceTorqueSensorTest : public ::testing::Test std::vector full_interface_names_; const std::vector fts_interface_names_ = {"force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"}; +======= + std::array force_values_ = {{1.1, 2.2, 3.3}}; + std::array torque_values_ = {{4.4, 5.5, 6.6}}; + std::unique_ptr force_torque_sensor_; + + std::vector full_interface_names_; + const std::vector fts_interface_names_ = { + {"force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"}}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; #endif // TEST_FORCE_TORQUE_SENSOR_HPP_ diff --git a/controller_interface/test/test_imu_sensor.hpp b/controller_interface/test/test_imu_sensor.hpp index 9d0a39e7e5..605c46a19e 100644 --- a/controller_interface/test/test_imu_sensor.hpp +++ b/controller_interface/test/test_imu_sensor.hpp @@ -19,12 +19,20 @@ #ifndef TEST_IMU_SENSOR_HPP_ #define TEST_IMU_SENSOR_HPP_ +<<<<<<< HEAD +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include +<<<<<<< HEAD #include "gmock/gmock.h" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "semantic_components/imu_sensor.hpp" // implementing and friending so we can access member variables @@ -56,9 +64,15 @@ class IMUSensorTest : public ::testing::Test protected: const size_t size_ = 10; const std::string sensor_name_ = "test_IMU"; +<<<<<<< HEAD std::array orientation_values_ = {1.1, 2.2, 3.3, 4.4}; std::array angular_velocity_values_ = {4.4, 5.5, 6.6}; std::array linear_acceleration_values_ = {4.4, 5.5, 6.6}; +======= + std::array orientation_values_ = {{1.1, 2.2, 3.3, 4.4}}; + std::array angular_velocity_values_ = {{4.4, 5.5, 6.6}}; + std::array linear_acceleration_values_ = {{4.4, 5.5, 6.6}}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::unique_ptr imu_sensor_; std::vector full_interface_names_; diff --git a/controller_interface/test/test_semantic_component_interface.hpp b/controller_interface/test/test_semantic_component_interface.hpp index 1996ddd193..83155ec400 100644 --- a/controller_interface/test/test_semantic_component_interface.hpp +++ b/controller_interface/test/test_semantic_component_interface.hpp @@ -19,11 +19,19 @@ #ifndef TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_ #define TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_ +<<<<<<< HEAD +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include "geometry_msgs/msg/wrench.hpp" +<<<<<<< HEAD #include "gmock/gmock.h" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "semantic_components/semantic_component_interface.hpp" // implementing and friending so we can access member variables diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index e3cd0c7312..e4b539478e 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- * Add CM `switch_controller` service timeout as parameter to spawner.py (backport `#1790 `_) (`#1879 `_) @@ -226,6 +227,379 @@ Changelog for package controller_manager * Don't ask to export reference interface if controller not 'inactive' or 'active' (`#824 `_) (`#843 `_) * Contributors: Denis Štogl, Tony Najjar +======= +4.21.0 (2024-12-06) +------------------- +* Use the .hpp headers from realtime_tools package (`#1916 `_) +* CM: Check if a valid time is received (`#1901 `_) +* Lock memory by default on a realtime system setup (`#1896 `_) +* Fix the launch_utils regression (`#1909 `_) +* [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (`#1871 `_) +* Add more parameter overriding tests by parsing multiple parameter files (`#1899 `_) +* add logic for 'params_file' to handle both string and string_array (`#1898 `_) +* [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) +* Add documentation on `ros2_control_node` and make lock_memory false by default (`#1890 `_) +* Add service call timeout argument in spawner (`#1808 `_) +* Add CM `switch_controller` service timeout as parameter to spawner.py (`#1790 `_) +* Fix the missing bcolors.ENDC in hardware_spawner log prints (`#1870 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Tony Najjar + +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 `_) +* [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 `_) +* [CM] Async Function Handler for Controllers (`#1489 `_) +* [Spawner] Add support for wildcard entries in the controller param files (`#1724 `_) +* [Feature] Fallback controllers (`#1789 `_) +* Check the update_rate set to the controllers to be a valid one (`#1788 `_) +* [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_) +* Improve diagnostics of Controllers, Hardware Components and Controller Manager (`#1764 `_) +* Improve launch utils to support the multiple controller names (`#1782 `_) +* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) +* [CM] Throw an exception when the components initially fail to be in the required state (`#1729 `_) +* Contributors: Felix Exner (fexner), Sai Kishor Kothakota + +4.18.0 (2024-10-07) +------------------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* Add test coverage for `params_file` parameter in spawner/unspawner tests (`#1754 `_) +* [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota, Santosh Govindaraj + +4.17.0 (2024-09-11) +------------------- +* Log exception type when catching the exception (`#1749 `_) +* [CM] Handle other exceptions while loading the controller plugin (`#1731 `_) +* remove unnecessary log of the CM args (`#1720 `_) +* Fix unload of controllers when spawned with `--unload-on-kill` (`#1717 `_) +* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + +4.16.1 (2024-08-24) +------------------- +* propage a portion of global args to the controller nodes (`#1712 `_) +* Contributors: Sai Kishor Kothakota + +4.16.0 (2024-08-22) +------------------- +* inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) +* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) +* Fix spawner tests timeout (`#1692 `_) +* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) +* Robustify controller spawner and add integration test with many controllers (`#1501 `_) +* Handle waiting in Spawner and align Hardware Spawner functionality (`#1562 `_) +* Make list controller and list hardware components immediately visualize the state. (`#1606 `_) +* [CI] Add coveragepy and remove ignore: test (`#1668 `_) +* Fix spawner unload on kill test (`#1675 `_) +* [CM] Add more logging for easier debugging (`#1645 `_) +* refactor SwitchParams to fix the incosistencies in the spawner tests (`#1638 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota + +4.15.0 (2024-08-05) +------------------- +* Add missing include for executors (`#1653 `_) +* Fix the namespaced controller_manager spawner + added tests (`#1640 `_) +* CM: Add missing includes (`#1641 `_) +* Fix rst markup (`#1642 `_) +* Add a pytest launch file to test ros2_control_node (`#1636 `_) +* [CM] Remove deprecated spawner args (`#1639 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Remove noqa (`#1626 `_) +* Fix controller starting with later load of robot description test (`#1624 `_) +* [CM] Remove support for the description parameter and use only `robot_description` topic (`#1358 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Henry Moore, Sai Kishor Kothakota + +4.13.0 (2024-07-08) +------------------- +* Change the spamming checking interface ERROR to DEBUG (`#1605 `_) +* [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) +* [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) +* Contributors: Sai Kishor Kothakota + +4.12.0 (2024-07-01) +------------------- +* [rqt_controller_manager] Add hardware components (`#1455 `_) +* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 `_) +* Fix update `period` for the first update after activation (`#1551 `_) +* Bump version of pre-commit hooks (`#1556 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, github-actions[bot] + +4.11.0 (2024-05-14) +------------------- +* Add find_package for ament_cmake_gen_version_h (`#1534 `_) +* Contributors: Christoph Fröhlich + +4.10.0 (2024-05-08) +------------------- +* allow extra spawner arguments to not declare every argument in launch utils (`#1505 `_) +* Working async controllers and components [not synchronized] (`#1041 `_) +* Add fallback controllers list to the ControllerInfo (`#1503 `_) +* Add a functionality to look for the controller type in the params file when not parsed (`#1502 `_) +* Add controller exception handling in controller manager (`#1507 `_) +* Contributors: Márk Szitanics, Sai Kishor Kothakota + +4.9.0 (2024-04-30) +------------------ +* Deactivate the controllers when they return error similar to hardware (`#1499 `_) +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.8.0 (2024-03-27) +------------------ +* generate version.h file per package using the ament_generate_version_header (`#1449 `_) +* Use ament_cmake generated rclcpp version header (`#1448 `_) +* Replace random_shuffle with shuffle. (`#1446 `_) +* Contributors: Chris Lalancette, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* add missing compiler definitions of RCLCPP_VERSION_MAJOR (`#1440 `_) +* Codeformat from new pre-commit config (`#1433 `_) +* add conditioning to get_parameter_value method import (`#1428 `_) +* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.6.0 (2024-03-02) +------------------ +* Add -Werror=missing-braces to compile options (`#1423 `_) +* added conditioning to have rolling tags compilable in older versions (`#1422 `_) +* [CM] Remove deprecated parameters for initial component states. (`#1357 `_) +* [BREAKING CHANGE] Use `robot_description` topic instead of `~/robot_description` and update docs regarding this (`#1410 `_) +* [CI] Code coverage + pre-commit (`#1413 `_) +* Fix multiple chainable controller activation bug (`#1401 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Sai Kishor Kothakota + +4.5.0 (2024-02-12) +------------------ +* check for state of the controller node before cleanup (`#1363 `_) +* [CM] Use explicit constants in controller tests. (`#1356 `_) +* [CM] Optimized debug output about interfaces when switching controllers. (`#1355 `_) +* A method to get node options to setup the controller node #api-breaking (`#1169 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.4.0 (2024-01-31) +------------------ +* Move `test_components` to own package (`#1325 `_) +* Fix controller parameter loading issue in different cases (`#1293 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.3.0 (2024-01-20) +------------------ +* [CM] Better readability and maintainability: rename variables, move code to more logical places 🔧 (`#1227 `_) +* Initialize the controller manager services after initializing resource manager (`#1271 `_) +* Issue 695: Changing 'namespace\_' variables to 'node_namespace' to make it more explicit (`#1239 `_) +* Fix rqt controller manager crash on ros2_control restart (`#1273 `_) +* [docs] Remove joint_state_controller (`#1263 `_) +* controller_manager: Add space to string literal concatenation (`#1245 `_) +* Try using SCHED_FIFO on any kernel (`#1142 `_) +* [CM] Set chained controller interfaces 'available' for activated controllers (`#1098 `_) +* [CM] Increase tests timeout (`#1224 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Sai Kishor Kothakota, Yasushi SHOJI, bailaC + +4.2.0 (2023-12-12) +------------------ +* [CM] Linting if/else statements (`#1193 `_) +* Reformat with braces added (`#1209 `_) +* Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_) +* Fix controller sorting issue while loading large number of controllers (`#1180 `_) +* Contributors: Bence Magyar, Dr. Denis, Lennart Nachtigall, Sai Kishor Kothakota + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* [ControllerManager] Fix all warnings from the latets features. (`#1174 `_) +* Compute the actual update period for each controller (`#1140 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* Fix the controller sorting bug when the interfaces are not configured (fixes `#1164 `_) (`#1165 `_) +* Pass URDF to controllers on init (`#1088 `_) +* Remove deprecation warning (`#1101 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota + +3.21.0 (2023-11-06) +------------------- +* Sort controllers while configuring instead of while activating (`#1107 `_) +* Contributors: Sai Kishor Kothakota + +3.20.0 (2023-10-31) +------------------- +* Update spawner to accept controllers list and start them in sequence (`#1139 `_) +* [ResourceManager] deactivate hardware from read/write return value (`#884 `_) +* Export of the get_cm_node_options() from robostack (`#1129 `_) +* Contributors: Felix Exner (fexner), Olivier Stasse, Sai Kishor Kothakota + +3.19.1 (2023-10-04) +------------------- +* Fix next controller update cycle time clock (`#1127 `_) +* Contributors: Sai Kishor Kothakota + +3.19.0 (2023-10-03) +------------------- +* Proper controller update rate (`#1105 `_) +* Fix multiple calls to export reference interfaces (`#1108 `_) +* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +3.18.0 (2023-08-17) +------------------- +* Controller sorting and proper execution in a chain (Fixes `#853 `_) (`#1063 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich, Dr Denis, Bence Magyar + +3.17.0 (2023-08-07) +------------------- +* [CM] Fixes the issue with individual controller's update rate (`#1082 `_) +* Fix deprecation warning (`#1093 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +3.16.0 (2023-07-09) +------------------- +* added controller manager runner to have update cycles (`#1075 `_) +* [CM] Make error message after trying to active controller more informative. (`#1066 `_) +* Fix equal and higher controller update rate (`#1070 `_) +* Create doc file for chained controllers (`#985 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +3.15.0 (2023-06-23) +------------------- +* Enable setting of initial state in HW compoments (`#1046 `_) +* [CM] Improve output when using robot description topic and give output about correct topic even remapped. (`#1059 `_) +* Contributors: Dr. Denis + +3.14.0 (2023-06-14) +------------------- +* Add -Wconversion flag to protect future developments (`#1053 `_) +* [CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (`#940 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#1037 `_) +* Docs: Use branch name substitution for all links (`#1031 `_) +* Add text to assertions references (`#1023 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota, gwalck + +3.13.0 (2023-05-18) +------------------- +* Add class for thread management of async hw interfaces (`#981 `_) +* Fix GitHub link on control.ros.org (`#1022 `_) +* Remove log-level argument from spawner script (`#1013 `_) +* Contributors: Christoph Fröhlich, Márk Szitanics, Bijou Abraham + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- +* [Controller Interface] Add time and period paramters to update_reference_from_subscribers() (`#846 `_) #API-break +* Contributors: Robotgir + +3.11.0 (2023-03-22) +------------------- +* [ControllerManager] Add Class for Async Controllers and Lifecycle Management (`#932 `_) +* Consistent use of colors for warning and error msgs in spawner (`#974 `_) +* Fix wrong warning messages (`#973 `_) +* Add log level support to spawner node (`#972 `_) +* Contributors: Dr. Denis, Márk Szitanics, Bijou Abraham + +3.10.0 (2023-03-16) +------------------- +* add spawner for hardware (`#941 `_) +* Contributors: Manuel Muth + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ +* fix AttributeError: Parameter object attribute name is read-only (`#957 `_) +* Remove deprecations from CLI and controller_manager (`#948 `_) +* Expose node options to controller manager (`#942 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García, methylDragon + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Add list_hardware_components CLI `_ - Adds list_hardware_components to CLI (`#891 `_) +* Contributors: Andy McEvoy, Dr. Denis, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ +* Do not use CLI calls but direct API for setting parameters. (`#910 `_) +* Optimize output of controller spawner (`#909 `_) +* ControllerManager: catch exception by reference (`#906 `_) +* Test fix: don't keep reference to the controller in the test when it should be destroyed in the controller manager (`#883 `_) +* Merge branch 'fix-update-rate' into humble (`#874 `_) +* Contributors: Christopher Wecht, Dr. Denis, Tony Najjar, sgmurray + +3.6.0 (2023-01-12) +------------------ +* Fix QoS deprecation warnings (`#879 `_) +* Add backward_ros to controller_manager (`#886 `_) +* Contributors: Andy McEvoy, Bence Magyar + +3.5.1 (2023-01-06) +------------------ +* Prevent controller manager from crashing when controller's plugin has error during loading. (`#881 `_) +* Contributors: Denis Štogl + +3.5.0 (2022-12-06) +------------------ +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Namespace Loaded Controllers (`#852 `_) +* Contributors: Bence Magyar, sp-sophia-labs + +3.4.0 (2022-11-27) +------------------ +* Use a thread priority library from realtime_tools (`#794 `_) +* [Doc] Correct type of update_rate parameter (`#858 `_) +* Contributors: Andy Zelenak, Denis Štogl, Bence Magyar + +3.3.0 (2022-11-15) +------------------ +* Adding activation/deactivation tests for chain controllers (`#809 `_) +* Fix const-ness in std::chrono::time_point construction and explicitly use std::chrono::nanoseconds as std::chrono::time_point template parameter to help compilation on macOS as its std::chrono::system_clock::time_point defaults to std::chrono::milliseconds for duration type (`#848 `_) +* [ControllerManager] Fix wrong initialization order and avoid compiler warnings (`#836 `_) +* Contributors: Adrian Zwiener, Bilal Gill, Felix Exner, light-tech + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ +* Don't ask to export reference interface if controller not 'inactive' or 'active' (`#824 `_) +* Add diagnostics for controllers activity (`#820 `_) +* Search for controller manager in the same namespace as spawner (`#810 `_) +* Handle HW errors on read and write in CM by stopping controllers (`#742 `_) + Add code for deactivating controller when hardware gets an error on read and write. + Fix misleading variable name in the tests. + Remove all interface from available list for hardware when an error happens. + Do some more variable renaming to the new nomenclature. +* Contributors: Denis Štogl, Tony Najjar + +3.0.0 (2022-09-19) +------------------ + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1a53d69bde..7ef254721a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(controller_manager) @@ -14,16 +15,43 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp realtime_tools std_msgs +======= +cmake_minimum_required(VERSION 3.16) +project(controller_manager LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Werror=missing-braces) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_index_cpp + controller_interface + controller_manager_msgs + diagnostic_updater + hardware_interface + pluginlib + rclcpp + realtime_tools + std_msgs + libstatistics_collector + generate_parameter_library +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(ament_cmake_core REQUIRED) +<<<<<<< HEAD +======= +find_package(ament_cmake_gen_version_h REQUIRED) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +<<<<<<< HEAD add_library(${PROJECT_NAME} SHARED src/controller_manager.cpp ) @@ -52,12 +80,39 @@ install(TARGETS ros2_control_node install(DIRECTORY include/ DESTINATION include +======= +generate_parameter_library(controller_manager_parameters + src/controller_manager_parameters.yaml +) + +add_library(controller_manager SHARED + src/controller_manager.cpp +) +target_compile_features(controller_manager PUBLIC cxx_std_17) +target_include_directories(controller_manager PUBLIC + $ + $ +) +target_link_libraries(controller_manager PUBLIC + controller_manager_parameters +) +ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + +add_executable(ros2_control_node src/ros2_control_node.cpp) +target_link_libraries(ros2_control_node PRIVATE + controller_manager +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) +<<<<<<< HEAD add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_include_directories(test_controller PRIVATE include) target_link_libraries(test_controller ${PROJECT_NAME}) @@ -174,11 +229,158 @@ if(BUILD_TESTING) target_include_directories(test_spawner_unspawner PRIVATE include) target_link_libraries(test_spawner_unspawner ${PROJECT_NAME} test_controller) ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets) +======= + # 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) + target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) + install( + TARGETS test_controller + DESTINATION lib + ) + + add_library(test_controller_failed_init SHARED + test/test_controller_failed_init/test_controller_failed_init.cpp + ) + target_link_libraries(test_controller_failed_init PUBLIC controller_manager) + target_compile_definitions(test_controller_failed_init PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller_failed_init/test_controller_failed_init.xml) + install( + TARGETS test_controller_failed_init + DESTINATION lib + ) + + add_library(test_chainable_controller SHARED + test/test_chainable_controller/test_chainable_controller.cpp + ) + ament_target_dependencies(test_chainable_controller PUBLIC realtime_tools) + target_link_libraries(test_chainable_controller PUBLIC controller_manager) + target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_chainable_controller/test_chainable_controller.xml) + install( + TARGETS test_chainable_controller + DESTINATION lib + ) + + ament_add_gmock(test_controller_manager + test/test_controller_manager.cpp + TIMEOUT 180 + ) + target_link_libraries(test_controller_manager + controller_manager + test_controller + test_chainable_controller + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_controller_manager_with_namespace + test/test_controller_manager_with_namespace.cpp + ) + target_link_libraries(test_controller_manager_with_namespace + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_controller_manager_hardware_error_handling + test/test_controller_manager_hardware_error_handling.cpp + ) + target_link_libraries(test_controller_manager_hardware_error_handling + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_load_controller + test/test_load_controller.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_link_libraries(test_load_controller + controller_manager + test_controller + test_controller_failed_init + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_controllers_chaining_with_controller_manager + test/test_controllers_chaining_with_controller_manager.cpp + ) + target_link_libraries(test_controllers_chaining_with_controller_manager + controller_manager + test_chainable_controller + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_controller_manager_srvs + test/test_controller_manager_srvs.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_link_libraries(test_controller_manager_srvs + controller_manager + test_controller + test_chainable_controller + ros2_control_test_assets::ros2_control_test_assets + ) + set_tests_properties(test_controller_manager_srvs PROPERTIES TIMEOUT 120) + ament_target_dependencies(test_controller_manager_srvs + controller_manager_msgs + ) + ament_add_gmock(test_controller_manager_urdf_passing + test/test_controller_manager_urdf_passing.cpp + ) + target_link_libraries(test_controller_manager_urdf_passing + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + ament_target_dependencies(test_controller_manager_urdf_passing + controller_manager_msgs + ) + + add_library(test_controller_with_interfaces SHARED + test/test_controller_with_interfaces/test_controller_with_interfaces.cpp + ) + target_link_libraries(test_controller_with_interfaces PUBLIC + controller_manager + ) + target_compile_definitions(test_controller_with_interfaces PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller_with_interfaces/test_controller_with_interfaces.xml) + install( + TARGETS test_controller_with_interfaces + DESTINATION lib + ) + + ament_add_gmock(test_release_interfaces + test/test_release_interfaces.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_link_libraries(test_release_interfaces + controller_manager + test_controller_with_interfaces + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_spawner_unspawner + test/test_spawner_unspawner.cpp + TIMEOUT 120 + ) + target_link_libraries(test_spawner_unspawner + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_add_gmock(test_hardware_spawner test/test_hardware_spawner.cpp TIMEOUT 120 ) +<<<<<<< HEAD target_include_directories(test_hardware_spawner PRIVATE include) target_link_libraries(test_hardware_spawner ${PROJECT_NAME}) ament_target_dependencies(test_hardware_spawner ros2_control_test_assets) @@ -188,10 +390,23 @@ if(BUILD_TESTING) install(FILES test/test_controller_spawner_with_basic_controllers.yaml DESTINATION test) +======= + 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) + + install(FILES test/test_controller_spawner_with_type.yaml + DESTINATION test) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) install(FILES test/test_controller_overriding_parameters.yaml DESTINATION test) +<<<<<<< HEAD ament_add_gmock( test_hardware_management_srvs test/test_hardware_management_srvs.cpp @@ -202,6 +417,21 @@ if(BUILD_TESTING) test_hardware_management_srvs controller_manager_msgs ros2_control_test_assets +======= + install(FILES test/test_controller_spawner_wildcard_entries.yaml + DESTINATION test) + + ament_add_gmock(test_hardware_management_srvs + test/test_hardware_management_srvs.cpp + ) + target_link_libraries(test_hardware_management_srvs + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + ament_target_dependencies(test_hardware_management_srvs + controller_manager_msgs +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) find_package(ament_cmake_pytest REQUIRED) @@ -210,6 +440,7 @@ if(BUILD_TESTING) ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) endif() +<<<<<<< HEAD # Install Python modules ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) @@ -222,5 +453,28 @@ ament_export_include_directories( ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +======= +install( + DIRECTORY include/ + DESTINATION include/controller_manager +) +install( + TARGETS controller_manager controller_manager_parameters + EXPORT export_controller_manager + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) +install( + TARGETS ros2_control_node + RUNTIME DESTINATION lib/controller_manager +) + +ament_python_install_package(controller_manager + SCRIPTS_DESTINATION lib/controller_manager +) +ament_export_targets(export_controller_manager HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_package() ament_generate_version_header(${PROJECT_NAME}) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a5148098f0..d9f62dc3ba 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -88,6 +88,7 @@ def service_caller( @return The service response """ +<<<<<<< HEAD cli = node.create_client(service_type, service_name) while not cli.service_is_ready(): @@ -97,6 +98,25 @@ def service_caller( raise ServiceNotFoundError(f"Could not contact service {service_name}") elif not cli.wait_for_service(10.0): node.get_logger().warn(f"Could not contact service {service_name}") +======= + namespace = "" if node.get_namespace() == "/" else node.get_namespace() + 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) + + while not cli.service_is_ready(): + node.get_logger().info( + f"waiting for service {fully_qualified_service_name} to become available..." + ) + if service_timeout: + if not cli.wait_for_service(service_timeout): + raise ServiceNotFoundError( + f"Could not contact service {fully_qualified_service_name}" + ) + elif not cli.wait_for_service(10.0): + node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}") +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) node.get_logger().debug(f"requester: making request: {request}\n") future = None @@ -105,17 +125,31 @@ def service_caller( rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) if future.result() is None: node.get_logger().warning( +<<<<<<< HEAD f"Failed getting a result from calling {service_name} in " +======= + f"Failed getting a result from calling {fully_qualified_service_name} in " +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) f"{call_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result() raise RuntimeError( +<<<<<<< HEAD f"Could not successfully call service {service_name} after {max_attempts} attempts." ) def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +======= + f"Could not successfully call service {fully_qualified_service_name} after {max_attempts} attempts." + ) + + +def configure_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -124,10 +158,18 @@ def configure_controller(node, controller_manager_name, controller_name, service ConfigureController, request, service_timeout, +<<<<<<< HEAD ) def list_controllers(node, controller_manager_name, service_timeout=0.0): +======= + call_timeout, + ) + + +def list_controllers(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = ListControllers.Request() return service_caller( node, @@ -135,10 +177,18 @@ def list_controllers(node, controller_manager_name, service_timeout=0.0): ListControllers, request, service_timeout, +<<<<<<< HEAD ) def list_controller_types(node, controller_manager_name, service_timeout=0.0): +======= + call_timeout, + ) + + +def list_controller_types(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = ListControllerTypes.Request() return service_caller( node, @@ -146,10 +196,20 @@ def list_controller_types(node, controller_manager_name, service_timeout=0.0): ListControllerTypes, request, service_timeout, +<<<<<<< HEAD ) def list_hardware_components(node, controller_manager_name, service_timeout=0.0): +======= + call_timeout, + ) + + +def list_hardware_components( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = ListHardwareComponents.Request() return service_caller( node, @@ -157,10 +217,20 @@ def list_hardware_components(node, controller_manager_name, service_timeout=0.0) ListHardwareComponents, request, service_timeout, +<<<<<<< HEAD ) def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): +======= + call_timeout, + ) + + +def list_hardware_interfaces( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = ListHardwareInterfaces.Request() return service_caller( node, @@ -168,10 +238,20 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0) ListHardwareInterfaces, request, service_timeout, +<<<<<<< HEAD ) def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +======= + call_timeout, + ) + + +def load_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = LoadController.Request() request.name = controller_name return service_caller( @@ -180,10 +260,20 @@ def load_controller(node, controller_manager_name, controller_name, service_time LoadController, request, service_timeout, +<<<<<<< HEAD ) def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): +======= + call_timeout, + ) + + +def reload_controller_libraries( + node, controller_manager_name, force_kill, service_timeout=0.0, call_timeout=10.0 +): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -192,11 +282,24 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ReloadControllerLibraries, request, service_timeout, +<<<<<<< HEAD +======= + call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) def set_hardware_component_state( +<<<<<<< HEAD node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 +======= + node, + controller_manager_name, + component_name, + lifecyle_state, + service_timeout=0.0, + call_timeout=10.0, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ): request = SetHardwareComponentState.Request() request.name = component_name @@ -206,6 +309,11 @@ def set_hardware_component_state( f"{controller_manager_name}/set_hardware_component_state", SetHardwareComponentState, request, +<<<<<<< HEAD +======= + service_timeout, + call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) @@ -217,6 +325,10 @@ def switch_controllers( strict, activate_asap, timeout, +<<<<<<< HEAD +======= + call_timeout=10.0, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ): request = SwitchController.Request() request.activate_controllers = activate_controllers @@ -228,11 +340,25 @@ def switch_controllers( request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() return service_caller( +<<<<<<< HEAD node, f"{controller_manager_name}/switch_controller", SwitchController, request ) def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +======= + node, + f"{controller_manager_name}/switch_controller", + SwitchController, + request, + call_timeout=call_timeout, + ) + + +def unload_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) request = UnloadController.Request() request.name = controller_name return service_caller( @@ -241,6 +367,10 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti UnloadController, request, service_timeout, +<<<<<<< HEAD +======= + call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) @@ -256,6 +386,10 @@ def get_params_files_with_controller_parameters( f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) WILDCARD_KEY = "/**" +<<<<<<< HEAD +======= + ROS_PARAMS_KEY = "ros__parameters" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) parameters = yaml.safe_load(f) # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' for key in [ @@ -264,6 +398,11 @@ def get_params_files_with_controller_parameters( f"{WILDCARD_KEY}/{controller_name}", f"{WILDCARD_KEY}{namespaced_controller}", ]: +<<<<<<< HEAD +======= + if parameter_file in controller_parameter_files: + break +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if key in parameters: if key == controller_name and namespace != "/": node.get_logger().fatal( @@ -274,6 +413,11 @@ def get_params_files_with_controller_parameters( if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: controller_parameter_files.append(parameter_file) +<<<<<<< HEAD +======= + if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]: + controller_parameter_files.append(parameter_file) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_parameter_files @@ -306,6 +450,11 @@ def get_parameter_from_param_files( if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: controller_param_dict = parameters[WILDCARD_KEY][key] +<<<<<<< HEAD +======= + if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if controller_param_dict and ( not isinstance(controller_param_dict, dict) @@ -394,4 +543,24 @@ def set_controller_parameters_from_param_files( node, controller_manager_name, controller_name, "type", controller_type ): return False +<<<<<<< HEAD +======= + + fallback_controllers = get_parameter_from_param_files( + node, + controller_name, + spawner_namespace, + controller_parameter_files, + "fallback_controllers", + ) + if fallback_controllers: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "fallback_controllers", + fallback_controllers, + ): + return False +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return True diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 823181a0ee..bb7e4e96b6 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -20,10 +20,14 @@ def generate_controllers_spawner_launch_description( +<<<<<<< HEAD controller_names: list, controller_type=None, controller_params_files=None, extra_spawner_args=[], +======= + controller_names: list, controller_params_files=None, extra_spawner_args=[] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ): """ Generate launch description for loading a controller using spawner. @@ -34,10 +38,17 @@ def generate_controllers_spawner_launch_description( Examples -------- +<<<<<<< HEAD # Assuming the controller type and controller parameters are known to the controller_manager generate_controllers_spawner_launch_description(['joint_state_broadcaster']) # Passing controller type and parameter file to load the controller +======= + # Assuming the controller parameters are known to the controller_manager + generate_controllers_spawner_launch_description(['joint_state_broadcaster']) + + # Passing controller parameter file to load the controller (Controller type is retrieved from config file) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) generate_controllers_spawner_launch_description( ['joint_state_broadcaster'], controller_params_files=[os.path.join(get_package_share_directory('my_pkg'), @@ -143,12 +154,19 @@ def generate_controllers_spawner_launch_description_from_dict( def generate_load_controller_launch_description( +<<<<<<< HEAD controller_name: str, controller_type=None, controller_params_file=None, extra_spawner_args=[] +======= + controller_name: str, controller_params_file=None, extra_spawner_args=[] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ): controller_params_files = [controller_params_file] if controller_params_file else None return generate_controllers_spawner_launch_description( controller_names=[controller_name], +<<<<<<< HEAD controller_type=controller_type, +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_params_files=controller_params_files, extra_spawner_args=extra_spawner_args, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 968a4d3d44..cc686970e9 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,7 +26,10 @@ load_controller, switch_controllers, unload_controller, +<<<<<<< HEAD set_controller_parameters, +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) set_controller_parameters_from_param_files, bcolors, ) @@ -61,13 +64,25 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) +<<<<<<< HEAD def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): controllers = list_controllers(node, controller_manager, service_timeout).controller +======= +def is_controller_loaded( + node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0 +): + controllers = list_controllers( + node, controller_manager, service_timeout, call_timeout + ).controller +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return any(c.name == controller_name for c in controllers) def main(args=None): +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() parser.add_argument("controller_names", help="List of controllers", nargs="+") @@ -89,7 +104,15 @@ def main(args=None): required=False, ) parser.add_argument( +<<<<<<< HEAD "-n", "--namespace", help="Namespace for the controller", default="", required=False +======= + "-n", + "--namespace", + help="DEPRECATED Namespace for the controller_manager and the controller(s)", + default=None, + required=False, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) parser.add_argument( "--load-only", @@ -98,18 +121,22 @@ def main(args=None): required=False, ) parser.add_argument( +<<<<<<< HEAD "--stopped", help="Load and configure the controller, however do not activate them", action="store_true", required=False, ) parser.add_argument( +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "--inactive", help="Load and configure the controller, however do not activate them", action="store_true", required=False, ) parser.add_argument( +<<<<<<< HEAD "-t", "--controller-type", help="If not provided it should exist in the controller manager namespace", @@ -117,6 +144,8 @@ def main(args=None): required=False, ) parser.add_argument( +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "-u", "--unload-on-kill", help="Wait until this application is interrupted and unload controller", @@ -124,7 +153,11 @@ def main(args=None): ) parser.add_argument( "--controller-manager-timeout", +<<<<<<< HEAD help="Time to wait for the controller manager", +======= + help="Time to wait for the controller manager service to be available", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) required=False, default=0.0, type=float, @@ -139,6 +172,16 @@ def main(args=None): type=float, ) parser.add_argument( +<<<<<<< HEAD +======= + "--service-call-timeout", + help="Time to wait for the service response from the controller manager", + required=False, + default=10.0, + type=float, + ) + parser.add_argument( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "--activate-as-group", help="Activates all the parsed controllers list together instead of one by one." " Useful for activating all chainable controllers altogether", @@ -152,6 +195,10 @@ def main(args=None): controller_manager_name = args.controller_manager param_files = args.param_file controller_manager_timeout = args.controller_manager_timeout +<<<<<<< HEAD +======= + service_call_timeout = args.service_call_timeout +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) switch_timeout = args.switch_timeout if param_files: @@ -167,6 +214,17 @@ def main(args=None): f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!" ) +<<<<<<< HEAD +======= + if args.namespace: + warnings.filterwarnings("always") + warnings.warn( + "The '--namespace' argument is deprecated and will be removed in future releases." + " Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=", + DeprecationWarning, + ) + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) spawner_namespace = args.namespace if args.namespace else node.get_namespace() if not spawner_namespace.startswith("/"): @@ -180,8 +238,18 @@ def main(args=None): try: for controller_name in controller_names: +<<<<<<< HEAD if is_controller_loaded( node, controller_manager_name, controller_name, controller_manager_timeout +======= + + if is_controller_loaded( + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ): node.get_logger().warn( bcolors.WARNING @@ -189,6 +257,7 @@ def main(args=None): + bcolors.ENDC ) else: +<<<<<<< HEAD if args.controller_type: if not set_controller_parameters( node, @@ -198,6 +267,8 @@ def main(args=None): args.controller_type, ): return 1 +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if param_files: if not set_controller_parameters_from_param_files( node, @@ -223,14 +294,28 @@ def main(args=None): ) if not args.load_only: +<<<<<<< HEAD ret = configure_controller(node, controller_manager_name, controller_name) +======= + ret = configure_controller( + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, + ) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if not ret.ok: node.get_logger().error( bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC ) return 1 +<<<<<<< HEAD if not args.stopped and not args.inactive and not args.activate_as_group: +======= + if not args.inactive and not args.activate_as_group: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ret = switch_controllers( node, controller_manager_name, @@ -239,6 +324,10 @@ def main(args=None): True, True, switch_timeout, +<<<<<<< HEAD +======= + service_call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) if not ret.ok: node.get_logger().error( @@ -254,7 +343,11 @@ def main(args=None): + bcolors.ENDC ) +<<<<<<< HEAD if not args.stopped and not args.inactive and args.activate_as_group: +======= + if not args.inactive and args.activate_as_group: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ret = switch_controllers( node, controller_manager_name, @@ -263,6 +356,10 @@ def main(args=None): True, True, switch_timeout, +<<<<<<< HEAD +======= + service_call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) if not ret.ok: node.get_logger().error( @@ -275,8 +372,11 @@ def main(args=None): + f"Configured and activated all the parsed controllers list : {controller_names}!" + bcolors.ENDC ) +<<<<<<< HEAD if args.stopped: node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if not args.unload_on_kill: return 0 @@ -286,7 +386,11 @@ def main(args=None): while True: time.sleep(1) except KeyboardInterrupt: +<<<<<<< HEAD if not args.stopped and not args.inactive: +======= + if not args.inactive: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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( @@ -297,6 +401,10 @@ def main(args=None): True, True, switch_timeout, +<<<<<<< HEAD +======= + service_call_timeout, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) if not ret.ok: node.get_logger().error( @@ -308,9 +416,12 @@ def main(args=None): f"Successfully deactivated controllers : {controller_names}" ) +<<<<<<< HEAD elif args.stopped: node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) unload_status = True for controller_name in controller_names: ret = unload_controller(node, controller_manager_name, controller_name) diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 15f0ec2899..7fc9fb320b 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -26,7 +26,10 @@ def main(args=None): +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclpy.init(args=args) parser = argparse.ArgumentParser() parser.add_argument("controller_names", help="Name of the controller", nargs="+") diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index c594e51a7b..6584f6870f 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -27,7 +27,11 @@ To describe the intent of this document, lets focus on the simple yet sufficient :alt: Example2 +<<<<<<< HEAD In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers. +======= +In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers as well as the 'robot_localization' controller. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Let's now imagine a use-case where we don't only want to run all those controllers as a group, but also flexibly add preceding steps. This means the following: @@ -37,9 +41,25 @@ This means the following: 2. Then "diff_drive_controller" is activated and attaches itself to the virtual input interfaces of PID controllers. PID controllers also get informed that they are working in chained mode and therefore disable their external interface through subscriber. Now we check if kinematics of differential robot is running properly. +<<<<<<< HEAD 3. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces. 4. If any of the controllers is deactivated, also all preceding controllers are deactivated. +======= + 3. Once the 'diff_drive_controller' is activated, it exposes the 'odom' state interfaces that is used by 'odom_publisher' as well as 'sensor_fusion' controllers. + The 'odom_publisher' controller is activated and attaches itself to the exported 'odom' state interfaces of 'diff_drive_controller'. + The 'sensor_fusion' controller is activated and attaches itself to the exported 'odom' state interfaces of 'diff_drive_controller' along with the 'imu' state interfaces. + 4. Once the 'sensor_fusion' controller is activated, it exposes the 'odom' state interfaces that is used by 'robot_localization' controller. + The 'robot_localization' controller is activated and attaches itself to the 'odom' state interfaces of 'sensor_fusion' controller. + Once activated, the 'robot_localization' controller exposes the 'actual_pose' state interfaces that is used by 'position_tracking' controller. + 5. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces and to the 'robot_localization' controller which provides the 'actual_pose' state interface. + 6. If any of the controllers is deactivated, also all preceding controllers needs to be deactivated. + +.. note:: + + Only the controllers that exposes the reference interfaces are switched to chained mode, when their reference interfaces are used by other controllers. When their reference interfaces are not used by the other controllers, they are switched back to get references from the subscriber. + However, the controllers that exposes the state interfaces are not triggered to chained mode, when their state interfaces are used by other controllers. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Implementation -------------- @@ -47,6 +67,7 @@ Implementation A Controller Base-Class: ChainableController ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual InterfaceConfiguration input_interface_configuration() const = 0`` method. This method should implement for each controller that **can be preceded** by another controller exporting all the input interfaces. For simplicity reasons, it is assumed for now that controller's all input interfaces are used. @@ -60,6 +81,36 @@ Inner Resource Management ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ After configuring a chainable controller, controller manager calls ``input_interface_configuration`` method and takes ownership over controller's input interfaces. +======= +A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual std::vector export_reference_interfaces() = 0`` method as well as ``virtual std::vector export_state_interfaces() = 0`` method. +This method should be implemented for each controller that **can be preceded** by another controller exporting all the reference/state interfaces. +For simplicity reasons, it is assumed for now that controller's all reference interfaces are used by other controller. However, the state interfaces exported by the controller, can be used by multiple controllers at the same time and with the combination they want. +Therefore, do not try to implement any exclusive combinations of reference interfaces, but rather write multiple controllers if you need exclusivity. + +The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained mode is activated or deactivated, e.g., deactivating subscribers. + +As an example, PID controllers export one virtual interface ``pid_reference`` and stop their subscriber ``/pid_reference`` when used in chained mode. 'diff_drive_controller' controller exports list of virtual interfaces ``/v_x``, ``/v_y``, and ``/w_z``, and stops subscribers from topics ``/cmd_vel`` and ``/cmd_vel_unstamped``. Its publishers can continue running. + +Nomenclature +^^^^^^^^^^^^^^ + +There are two types of interfaces within the context of ``ros2_control``: ``CommandInterface`` and ``StateInterface``. + +- The ``CommandInterface`` is a Read-Write type of interface that can be used to get and set values. Its typical use-case is to set command values to the hardware. +- The ``StateInterface`` is a Read-Only type of interface that can be used to get values. Its typical use-case is to get actual state values from the hardware. + +These interfaces are utilized in different places within the controller in order to have a functional controller or controller chain that commands the hardware. + +- The ``virtual InterfaceConfiguration command_interface_configuration() const`` method defined in the ``ControllerInterface`` class is used to define the command interfaces used by the controller. These interfaces are used to command the hardware or the exposed reference interfaces from another controller. The ``controller_manager`` uses this configuration to claim the command interfaces from the ``ResourceManager``. +- The ``virtual InterfaceConfiguration state_interface_configuration() const`` method defined in the ``ControllerInterface`` class is used to define the state interfaces used by the controller. These interfaces are used to get the actual state values from the hardware or the exposed state interfaces from another controller. The ``controller_manager`` uses this configuration to claim the state interfaces from the ``ResourceManager``. +- The ``std::vector export_reference_interfaces()`` method defined in the ``ChainableController`` class is used to define the reference interfaces exposed by the controller. These interfaces are used to command the controller from other controllers. +- The ``std::vector export_state_interfaces()`` method defined in the ``ChainableController`` class is used to define the state interfaces exposed by the controller. These interfaces are used to get the actual state values from the controller by other controllers. + +Inner Resource Management +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +After configuring a chainable controller, controller manager calls ``export_reference_interfaces`` and ``export_state_interfaces`` method and takes ownership over controller's exported reference/state interfaces. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) This is the same process as done by ``ResourceManager`` and hardware interfaces. Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``). @@ -71,15 +122,30 @@ Chained controllers must be activated and deactivated together or in the proper This means you must first activate all following controllers to have the preceding one activated. For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated. One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle. +<<<<<<< HEAD +======= +The chained controllers can also be activated when parsed as in a single list through the fields ``activate_controllers`` or ``deactivate_controllers`` in the ``switch_controllers`` service provided by the controller_manager. +The controller_manager ``spawner`` can also be used to activate all the controllers of the chain in a single call, by parsing the argument ``--activate-as-group``. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Debugging outputs ---------------------------- +<<<<<<< HEAD Flag ``unavailable`` if the reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irrelevant for the usage. +======= +- The reference interfaces are ``unavailable`` and ``unclaimed``, when the controller exporting them is in inactive state +- The reference interfaces are ``available`` and ``unclaimed``, when the controller exporting them is in an active state but is not in chained mode with any other controller (The controllers gets its references from the subscriber) +- The reference interfaces are ``available`` and ``claimed``, when the controller exporting them is in active state and also in chained mode with other controllers (The controller gets its references from the controllers it is chained with) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Closing remarks ---------------------------- +<<<<<<< HEAD - Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add an implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. +======= +- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``export_reference_interfaces()`` and ``export_state_interfaces()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml new file mode 100644 index 0000000000..a015765c79 --- /dev/null +++ b/controller_manager/doc/parameters_context.yaml @@ -0,0 +1,21 @@ +hardware_components_initial_state: | + Map of parameters for controlled lifecycle management of hardware components. + The names of the components are defined as attribute of ````-tag in ``robot_description``. + Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. + Detailed explanation of each parameter is given below. + The full structure of the map is given in the following example: + + .. code-block:: yaml + + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +diagnostics.threshold.controllers.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.controllers.execution_time: | + The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle. diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 27ded48e5e..c1d7911050 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -11,10 +11,16 @@ Determinism ----------- For best performance when controlling hardware you want the controller manager to have as little jitter as possible in the main control loop. +<<<<<<< HEAD The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control. The two easiest kernel options are the `Real-time Ubuntu 22.04 LTS Beta `_ or `linux-image-rt-amd64 `_ on Debian Bullseye. If you have a realtime kernel installed, the main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``. +======= + +Independent of the kernel installed, the main thread of Controller Manager attempts to +configure ``SCHED_FIFO`` with a priority of ``50``. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) By default, the user does not have permission to set such a high priority. To give the user such permissions, add a group named realtime and add the user controlling your robot to this group: @@ -36,6 +42,7 @@ Afterwards, add the following limits to the realtime group in ``/etc/security/li The limits will be applied after you log out and in again. +<<<<<<< HEAD Parameters ----------- @@ -69,11 +76,101 @@ update_rate (mandatory; double) The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controller and writes commands to hardware. +======= +The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control. +Alternatives to the standard kernel include + +- `Real-time Ubuntu 22.04 LTS Beta `_ on Ubuntu 22.04 +- `linux-image-rt-amd64 `_ on Debian Bullseye +- lowlatency kernel (``sudo apt install linux-lowlatency``) on any ubuntu + +Though installing a realtime-kernel will definitely get the best results when it comes to low +jitter, using a lowlatency kernel can improve things a lot with being really easy to install. + +Subscribers +----------- + +robot_description [std_msgs::msg::String] + String with the URDF xml, e.g., from ``robot_state_publisher``. + Reloading of the URDF is not supported yet. + All joints defined in the ````-tag have to be present in the URDF. + + +Parameters +----------- +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. +<<<<<<< HEAD +======= +.params_file + The absolute path to the YAML file with parameters for the controller. + The file should contain the parameters for the controller in the standard ROS 2 YAML format. + +.fallback_controllers + List of controllers that are activated as a fallback strategy, when the spawned controllers fail by returning ``return_type::ERROR`` during the ``update`` cycle. + It is recommended to add all the controllers needed for the fallback strategy to the list, including the chainable controllers whose interfaces are used by the main fallback controllers. + +.. warning:: + The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. + It is recommended to test the fallback strategy in simulation before deploying it on the real robot. + +.. generate_parameter_library_details:: + ../src/controller_manager_parameters.yaml + parameters_context.yaml + +**An example parameter file:** + +.. generate_parameter_library_default:: + ../src/controller_manager_parameters.yaml + + +Handling Multiple Controller Managers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +When dealing with multiple controller managers, you have two options for managing different robot descriptions: + +1. **Using Namespaces:** You can place both the ``robot_state_publisher`` and the ``controller_manager`` nodes into the same namespace. + +.. code-block:: python + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + namespace="rrbot", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + namespace="rrbot", + ) + +2. **Using Remappings:** You can use remappings to handle different robot descriptions. This involves relaying topics using the ``remappings`` tag, allowing you to specify custom topics for each controller manager. + +.. code-block:: python + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + remappings=[('robot_description', '/rrbot/robot_description')] + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + namespace="rrbot", + ) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Helper scripts -------------- @@ -90,8 +187,13 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h +<<<<<<< HEAD usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--stopped] [--inactive] [-t CONTROLLER_TYPE] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] +======= + 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] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_names [controller_names ...] positional arguments: @@ -104,6 +206,7 @@ There are two scripts to interact with controller manager from launch files: -p PARAM_FILE, --param-file PARAM_FILE Controller param file to be loaded into controller node before configure. Pass multiple times to load different files for different controllers or to override the parameters of the same controller. -n NAMESPACE, --namespace NAMESPACE +<<<<<<< HEAD Namespace for the controller --load-only Only load the controller and leave unconfigured. --stopped Load and configure the controller, however do not activate them @@ -118,12 +221,27 @@ There are two scripts to interact with controller manager from launch files: 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 +======= + DEPRECATED Namespace for the controller_manager and the controller(s) + --load-only Only load the controller and leave unconfigured. + --inactive Load and configure the controller, however do not activate them + -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 service to be available + --service-call-timeout SERVICE_CALL_TIMEOUT + Time to wait for the service response from 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 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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: .. code-block:: yaml +<<<<<<< HEAD /**/position_trajectory_controller: ros__parameters: type: joint_trajectory_controller/JointTrajectoryController @@ -134,10 +252,43 @@ The parsed controller config file can follow the same conventions as the typical command_interfaces: - position ..... +======= + /**: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + + command_interfaces: + - position + ..... + + position_trajectory_controller_joint1: + ros__parameters: + joints: + - joint1 + + position_trajectory_controller_joint2: + ros__parameters: + joints: + - joint2 + + .. code-block:: yaml + + /**/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .. code-block:: yaml /position_trajectory_controller: +<<<<<<< HEAD ros__parameters: type: joint_trajectory_controller/JointTrajectoryController joints: @@ -147,10 +298,22 @@ The parsed controller config file can follow the same conventions as the typical command_interfaces: - position ..... +======= + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .. code-block:: yaml position_trajectory_controller: +<<<<<<< HEAD ros__parameters: type: joint_trajectory_controller/JointTrajectoryController joints: @@ -160,10 +323,22 @@ The parsed controller config file can follow the same conventions as the typical command_interfaces: - position ..... +======= + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .. code-block:: yaml /rrbot_1/position_trajectory_controller: +<<<<<<< HEAD ros__parameters: type: joint_trajectory_controller/JointTrajectoryController joints: @@ -173,6 +348,17 @@ The parsed controller config file can follow the same conventions as the typical command_interfaces: - position ..... +======= + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ``unspawner`` ^^^^^^^^^^^^^^^^ @@ -216,22 +402,34 @@ The parsed controller config file can follow the same conventions as the typical --activate Activates the given components. Note: Components are by default configured before activated. --configure Configures the given components. +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rqt_controller_manager ---------------------- A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. .. image:: images/rqt_controller_manager.png +<<<<<<< HEAD It can be launched independently using the following command or as rqt plugin. +======= +It can be launched independently using the following command or as rqt plugin: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .. code-block:: console ros2 run rqt_controller_manager rqt_controller_manager +<<<<<<< HEAD * Double-click on a controller or hardware component to show the additional info. * Right-click on a controller or hardware component to show a context menu with options for lifecycle management. +======= +* Double-click on a controller or hardware component to show the additional info. +* Right-click on a controller or hardware component to show a context menu with options for lifecycle management. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Using the Controller Manager in a Process ----------------------------------------- @@ -284,9 +482,16 @@ lock_memory (optional; bool; default: false for a non-realtime kernel, true for Find more information about the setup for memory locking in the following link : `How to set ulimit values `_ The following command can be used to set the memory locking limit temporarily : ``ulimit -l unlimited``. +<<<<<<< HEAD cpu_affinity (optional; int; default: -1) Sets the CPU affinity of the ``controller_manager`` node to the specified CPU core. The value of -1 means that the CPU affinity is not set. +======= +cpu_affinity (optional; int (or) int_array;) + Sets the CPU affinity of the ``controller_manager`` node to the specified CPU core. + If it is an integer, the node's affinity will be set to the specified CPU core. + If it is an array of integers, the node's affinity will be set to the specified set of CPU cores. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) thread_priority (optional; int; default: 50) Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99. @@ -306,6 +511,16 @@ Note that not all controllers have to be restarted, e.g., broadcasters. Restarting hardware ^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD If hardware gets restarted then you should go through its lifecycle again. This can be simply achieved by returning ``ERROR`` from ``write`` and ``read`` methods of interface implementation. **NOT IMPLEMENTED YET - PLEASE STOP/RESTART ALL CONTROLLERS MANUALLY FOR NOW** The controller manager detects that and stops all the controllers that are commanding that hardware and restarts broadcasters that are listening to its states. +======= +If hardware gets restarted then you should go through its lifecycle again in order to reconfigure and export the interfaces + +Hardware and Controller Errors +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces. +Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4f28bf933b..3bbf44c1db 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -18,7 +18,10 @@ #include #include #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include @@ -40,21 +43,33 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" +<<<<<<< HEAD #include "hardware_interface/handle.hpp" +======= +#include "diagnostic_updater/diagnostic_updater.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/node.hpp" +<<<<<<< HEAD #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/rclcpp.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "std_msgs/msg/string.hpp" namespace controller_manager { +<<<<<<< HEAD +======= +class ParamListener; +class Params; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using ControllersListIterator = std::vector::const_iterator; CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); @@ -70,14 +85,29 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", +<<<<<<< HEAD const std::string & namespace_ = "", +======= + const std::string & node_namespace = "", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", +<<<<<<< HEAD const std::string & namespace_ = "", +======= + const std::string & node_namespace = "", + const rclcpp::NodeOptions & options = get_cm_node_options()); + + CONTROLLER_MANAGER_PUBLIC + ControllerManager( + std::shared_ptr executor, const std::string & urdf, + bool activate_all_hw_components, const std::string & manager_node_name = "controller_manager", + const std::string & node_namespace = "", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC @@ -121,6 +151,16 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; +<<<<<<< HEAD +======= + controller_spec.last_update_cycle_time = std::make_shared(0); + return add_controller_impl(controller_spec); + } + + controller_interface::ControllerInterfaceBaseSharedPtr add_controller( + const ControllerSpec & controller_spec) + { +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return add_controller_impl(controller_spec); } @@ -133,17 +173,29 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC controller_interface::return_type configure_controller(const std::string & controller_name); +<<<<<<< HEAD /// switch_controller Stops some controllers and start others. /** * \param[in] start_controllers is a list of controllers to start * \param[in] stop_controllers is a list of controllers to stop +======= + /// switch_controller Deactivates some controllers and activates others. + /** + * \param[in] activate_controllers is a list of controllers to activate. + * \param[in] deactivate_controllers is a list of controllers to deactivate. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * \param[in] set level of strictness (BEST_EFFORT or STRICT) * \see Documentation in controller_manager_msgs/SwitchController.srv */ CONTROLLER_MANAGER_PUBLIC controller_interface::return_type switch_controller( +<<<<<<< HEAD const std::vector & start_controllers, const std::vector & stop_controllers, int strictness, +======= + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, int strictness, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) bool activate_asap = kWaitForAllResources, const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); @@ -190,10 +242,43 @@ class ControllerManager : public rclcpp::Node // the executor (see issue #260). // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_; +<<<<<<< HEAD // Per controller update rate support CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; +======= + /// Interface for external components to check if Resource Manager is initialized. + /** + * Checks if components in Resource Manager are loaded and initialized. + * \returns true if they are initialized, false otherwise. + */ + CONTROLLER_MANAGER_PUBLIC + bool is_resource_manager_initialized() const + { + return resource_manager_ && resource_manager_->are_components_initialized(); + } + + /// Update rate of the main control loop in the controller manager. + /** + * Update rate of the main control loop in the controller manager. + * The method is used for per-controller update rate support. + * + * \returns update rate of the controller manager. + */ + 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(); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -205,8 +290,23 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void manage_switch(); +<<<<<<< HEAD CONTROLLER_MANAGER_PUBLIC void deactivate_controllers(); +======= + /// Deactivate chosen controllers from real-time controller list. + /** + * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_deactivate names of the controller that have to be deactivated. + */ + CONTROLLER_MANAGER_PUBLIC + void deactivate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_deactivate); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Switch chained mode for all the controllers with respect to the following cases: @@ -220,11 +320,42 @@ class ControllerManager : public rclcpp::Node void switch_chained_mode( const std::vector & chained_mode_switch_list, bool to_chained_mode); +<<<<<<< HEAD CONTROLLER_MANAGER_PUBLIC void activate_controllers(); CONTROLLER_MANAGER_PUBLIC void activate_controllers_asap(); +======= + /// Activate chosen controllers from real-time controller list. + /** + * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_activate names of the controller that have to be activated. + */ + CONTROLLER_MANAGER_PUBLIC + void activate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate); + + /// Activate chosen controllers from real-time controller list. + /** + * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * *NOTE*: There is currently not difference to `activate_controllers` method. + * Check https://github.com/ros-controls/ros2_control/issues/263 for more information. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_activate names of the controller that have to be activated. + */ + CONTROLLER_MANAGER_PUBLIC + void activate_controllers_asap( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CONTROLLER_MANAGER_PUBLIC void list_controllers_srv_cb( @@ -278,7 +409,11 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; +<<<<<<< HEAD unsigned int update_rate_ = 100; +======= + unsigned int update_rate_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::vector> chained_controllers_configuration_; std::unique_ptr resource_manager_; @@ -287,7 +422,13 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); +<<<<<<< HEAD void subscribe_to_robot_description_topic(); +======= + void init_controller_manager(); + + void initialize_parameters(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -315,7 +456,11 @@ class ControllerManager : public rclcpp::Node * * For each controller the whole chain of following controllers is checked. * +<<<<<<< HEAD * NOTE: The automatically adding of following controller into starting list is not implemented +======= + * NOTE: The automatically adding of following controller into activate list is not implemented +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * yet. * * \param[in] controllers list with controllers. @@ -339,7 +484,11 @@ class ControllerManager : public rclcpp::Node * - will be deactivated, * - and will not be activated. * +<<<<<<< HEAD * NOTE: The automatically adding of preceding controllers into stopping list is not implemented +======= + * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * yet. * * \param[in] controllers list with controllers. @@ -355,6 +504,7 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); +<<<<<<< HEAD /// A method to be used in the std::sort method to sort the controllers to be able to /// execute them in a proper order /** @@ -377,6 +527,48 @@ class ControllerManager : public rclcpp::Node bool controller_sorting( const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers); +======= + /// Checks if the fallback controllers of the given controllers are in the right + /// state, so they can be activated immediately + /** + * \param[in] controllers is a list of controllers to activate. + * \param[in] controller_it is the iterator pointing to the controller to be activated. + * \return return_type::OK if all fallback controllers are in the right state, otherwise + * return_type::ERROR. + */ + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it); + + /** + * @brief Inserts a controller into an ordered list based on dependencies to compute the + * controller chain. + * + * This method computes the controller chain by inserting the provided controller name into an + * ordered list of controllers based on dependencies. It ensures that controllers are inserted in + * the correct order so that dependencies are satisfied. + * + * @param ctrl_name The name of the controller to be inserted into the chain. + * @param controller_iterator An iterator pointing to the position in the ordered list where the + * controller should be inserted. + * @param append_to_controller Flag indicating whether the controller should be appended or + * prepended to the parsed iterator. + * @note The specification of controller dependencies is in the ControllerChainSpec, + * containing information about following and preceding controllers. This struct should include + * the neighboring controllers with their relationships to the provided controller. + * `following_controllers` specify controllers that come after the provided controller. + * `preceding_controllers` specify controllers that come before the provided controller. + */ + void update_list_with_controller_chain( + const std::string & ctrl_name, std::vector::iterator controller_iterator, + bool append_to_controller); + + void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + + void hardware_components_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + + void controller_manager_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * @brief determine_controller_node_options - A method that retrieves the controller defined node @@ -387,6 +579,13 @@ class ControllerManager : public rclcpp::Node */ rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; +<<<<<<< HEAD +======= + std::shared_ptr cm_param_listener_; + std::shared_ptr params_; + diagnostic_updater::Updater diagnostics_updater_; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::shared_ptr executor_; std::shared_ptr> loader_; @@ -476,6 +675,11 @@ class ControllerManager : public rclcpp::Node }; RTControllerListWrapper rt_controllers_wrapper_; +<<<<<<< HEAD +======= + std::unordered_map controller_chain_spec_; + std::vector ordered_controllers_names_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// mutex copied from ROS1 Control, protects service callbacks /// not needed if we're guaranteed that the callbacks don't come from multiple threads std::mutex services_lock_; @@ -505,7 +709,19 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; +<<<<<<< HEAD rclcpp::Subscription::SharedPtr robot_description_subscription_; +======= + std::map> controller_chained_reference_interfaces_cache_; + std::map> controller_chained_state_interfaces_cache_; + + rclcpp::NodeOptions cm_node_options_; + std::string robot_description_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; + rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + + controller_manager::MovingAverageStatistics periodicity_stats_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) struct SwitchParams { diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 63823f8d33..22c3f0955f 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -19,6 +19,7 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +<<<<<<< HEAD #include #include #include @@ -27,6 +28,20 @@ namespace controller_manager { +======= +#include +#include +#include +#include "controller_interface/controller_interface_base.hpp" +#include "hardware_interface/controller_info.hpp" +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" + +namespace controller_manager +{ + +using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -35,9 +50,31 @@ namespace controller_manager */ struct ControllerSpec { +<<<<<<< HEAD hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; }; +======= + ControllerSpec() + { + last_update_cycle_time = std::make_shared(0, 0, RCL_CLOCK_UNINITIALIZED); + execution_time_statistics = std::make_shared(); + periodicity_statistics = std::make_shared(); + } + + hardware_interface::ControllerInfo info; + controller_interface::ControllerInterfaceBaseSharedPtr c; + std::shared_ptr last_update_cycle_time; + std::shared_ptr execution_time_statistics; + std::shared_ptr periodicity_statistics; +}; + +struct ControllerChainSpec +{ + std::vector following_controllers; + std::vector preceding_controllers; +}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace controller_manager #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 48290c16c5..5ac23e385c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,11 @@ controller_manager +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Description of controller_manager Bence Magyar Denis Štogl @@ -16,6 +20,10 @@ backward_ros controller_interface controller_manager_msgs +<<<<<<< HEAD +======= + diagnostic_updater +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_interface launch launch_ros @@ -27,6 +35,11 @@ ros2param ros2run std_msgs +<<<<<<< HEAD +======= + libstatistics_collector + generate_parameter_library +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_cmake_gmock ament_cmake_pytest diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1f387b89ee..1192130a8e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,8 +14,13 @@ #include "controller_manager/controller_manager.hpp" +<<<<<<< HEAD #include #include +======= +#include +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include @@ -25,9 +30,17 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rcl/arguments.h" +<<<<<<< HEAD #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" +======= +#include "rclcpp/version.h" +#include "rclcpp_lifecycle/state.hpp" + +#include "controller_manager_parameters.hpp" + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace // utility { static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; @@ -37,7 +50,18 @@ static constexpr const char * kChainableControllerInterfaceClassName = "controller_interface::ChainableControllerInterface"; // Changed services history QoS to keep all so we don't lose any client service calls +<<<<<<< HEAD static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { +======= +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1, // message queue depth RMW_QOS_POLICY_RELIABILITY_RELIABLE, @@ -47,10 +71,19 @@ static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false}; +<<<<<<< HEAD inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; +======= +#endif + +inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) +{ + return controller.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } inline bool is_controller_inactive( @@ -61,7 +94,11 @@ inline bool is_controller_inactive( inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller) { +<<<<<<< HEAD return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; +======= + return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } inline bool is_controller_active( @@ -75,11 +112,19 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const return a.info.name == name; } +<<<<<<< HEAD /// Checks if a command interface belongs to a controller based on its prefix. /** * A command interface can be provided by a controller in which case is called "reference" * interface. * This means that the @interface_name starts with the name of a controller. +======= +/// Checks if an interface belongs to a controller based on its prefix. +/** + * A State/Command interface can be provided by a controller in which case is called + * "state/reference" interface. This means that the @interface_name starts with the name of a + * controller. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * * \param[in] interface_name to be found in the map. * \param[in] controllers list of controllers to compare their names to interface's prefix. @@ -87,7 +132,11 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const * @interface_name belongs to. * \return true if interface has a controller name as prefix, false otherwise. */ +<<<<<<< HEAD bool command_interface_is_reference_interface_of_controller( +======= +bool is_interface_a_chained_interface( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const std::string interface_name, const std::vector & controllers, controller_manager::ControllersListIterator & following_controller_it) @@ -117,14 +166,20 @@ bool command_interface_is_reference_interface_of_controller( { RCLCPP_DEBUG( rclcpp::get_logger("ControllerManager::utils"), +<<<<<<< HEAD "Required command interface '%s' with prefix '%s' is not reference interface.", interface_name.c_str(), interface_prefix.c_str()); +======= + "Required interface '%s' with prefix '%s' is not a chain interface.", interface_name.c_str(), + interface_prefix.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return false; } return true; } +<<<<<<< HEAD /** * A method to retrieve the names of all it's following controllers given a controller name * For instance, for the following case @@ -245,6 +300,77 @@ std::vector get_preceding_controller_names( } } return preceding_controllers; +======= +template +void add_element_to_list(std::vector & list, const T & element) +{ + if (std::find(list.begin(), list.end(), element) == list.end()) + { + // Only add to the list if it doesn't exist + list.push_back(element); + } +} + +template +void remove_element_from_list(std::vector & list, const T & element) +{ + auto itr = std::find(list.begin(), list.end(), element); + if (itr != list.end()) + { + list.erase(itr); + } +} + +void controller_chain_spec_cleanup( + std::unordered_map & ctrl_chain_spec, + const std::string & controller) +{ + const auto following_controllers = ctrl_chain_spec[controller].following_controllers; + const auto preceding_controllers = ctrl_chain_spec[controller].preceding_controllers; + for (const auto & flwg_ctrl : following_controllers) + { + remove_element_from_list(ctrl_chain_spec[flwg_ctrl].preceding_controllers, controller); + } + for (const auto & preced_ctrl : preceding_controllers) + { + remove_element_from_list(ctrl_chain_spec[preced_ctrl].following_controllers, controller); + } + ctrl_chain_spec.erase(controller); +} + +// Gets the list of active controllers that use the command interface of the given controller +void get_active_controllers_using_command_interfaces_of_controller( + const std::string & controller_name, + const std::vector & controllers, + std::vector & controllers_using_command_interfaces) +{ + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (it == controllers.end()) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the list of controllers.", controller_name.c_str()); + return; + } + const auto cmd_itfs = it->c->command_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + for (const auto & controller : controllers) + { + const auto ctrl_cmd_itfs = controller.c->command_interface_configuration().names; + // check if the controller is active and has the command interface and make sure that it + // doesn't exist in the list already + if ( + is_controller_active(controller.c) && + std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end()) + { + add_element_to_list(controllers_using_command_interfaces, controller.info.name); + } + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } // namespace @@ -257,19 +383,35 @@ rclcpp::NodeOptions get_cm_node_options() // Required for getting types of controllers to be loaded via service call node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); +<<<<<<< HEAD +======= +// \note The versions conditioning is added here to support the source-compatibility until Humble +#if RCLCPP_VERSION_MAJOR >= 21 + node_options.enable_logger_service(true); +#endif +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return node_options; } ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, +<<<<<<< HEAD const std::string & namespace_, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, namespace_, options), resource_manager_(std::make_unique()), +======= + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), + resource_manager_(std::make_unique( + this->get_node_clock_interface(), this->get_node_logging_interface())), + diagnostics_updater_(this), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) executor_(executor), loader_(std::make_shared>( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( +<<<<<<< HEAD kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { if (!get_parameter("update_rate", update_rate_)) @@ -292,19 +434,55 @@ ControllerManager::ControllerManager( init_resource_manager(robot_description); init_services(); } +======= + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) +{ + initialize_parameters(); + init_controller_manager(); +} + +ControllerManager::ControllerManager( + std::shared_ptr executor, const std::string & urdf, + bool activate_all_hw_components, const std::string & manager_node_name, + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), + diagnostics_updater_(this), + executor_(executor), + loader_(std::make_shared>( + kControllerInterfaceNamespace, kControllerInterfaceClassName)), + chainable_loader_( + std::make_shared>( + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) +{ + initialize_parameters(); + resource_manager_ = std::make_unique( + urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), + activate_all_hw_components, params_->update_rate); + init_controller_manager(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, +<<<<<<< HEAD const std::string & namespace_, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, namespace_, options), resource_manager_(std::move(resource_manager)), +======= + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), + resource_manager_(std::move(resource_manager)), + diagnostics_updater_(this), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) executor_(executor), loader_(std::make_shared>( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( +<<<<<<< HEAD kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { if (!get_parameter("update_rate", update_rate_)) @@ -331,10 +509,77 @@ void ControllerManager::subscribe_to_robot_description_topic() robot_description_subscription_ = create_subscription( "~/robot_description", rclcpp::QoS(1).transient_local(), std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); +======= + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) +{ + initialize_parameters(); + init_controller_manager(); +} + +void ControllerManager::init_controller_manager() +{ + // Get parameters needed for RT "update" loop to work + if (is_resource_manager_initialized()) + { + init_services(); + } + else + { + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN( + get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); + }); + } + + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + robot_description_subscription_ = create_subscription( + "robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); + RCLCPP_INFO( + get_logger(), "Subscribing to '%s' topic for robot description.", + robot_description_subscription_->get_topic_name()); + + // Setup diagnostics + periodicity_stats_.Reset(); + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + diagnostics_updater_.add( + "Hardware Components Activity", this, + &ControllerManager::hardware_components_diagnostic_callback); + diagnostics_updater_.add( + "Controller Manager Activity", this, + &ControllerManager::controller_manager_diagnostic_callback); +} + +void ControllerManager::initialize_parameters() +{ + // Initialize parameters + try + { + cm_param_listener_ = std::make_shared( + this->get_node_parameters_interface(), this->get_logger()); + params_ = std::make_shared(cm_param_listener_->get_params()); + update_rate_ = static_cast(params_->update_rate); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + this->get_logger(), + "Exception thrown while initializing controller manager parameters: %s \n", e.what()); + throw e; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { +<<<<<<< HEAD RCLCPP_INFO(get_logger(), "Received robot description file."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); @@ -360,13 +605,45 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & "The published robot description file (urdf) seems not to be genuine. The following error " "was caught:" << e.what()); +======= + RCLCPP_INFO(get_logger(), "Received robot description from topic."); + RCLCPP_DEBUG( + get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); + robot_description_ = robot_description.data; + if (is_resource_manager_initialized()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description."); + return; + } + init_resource_manager(robot_description_); + if (is_resource_manager_initialized()) + { + RCLCPP_INFO( + get_logger(), + "Resource Manager has been successfully initialized. Starting Controller Manager " + "services..."); + init_services(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } void ControllerManager::init_resource_manager(const std::string & robot_description) { +<<<<<<< HEAD // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... resource_manager_->load_urdf(robot_description); +======= + if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_)) + { + RCLCPP_WARN( + get_logger(), + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + return; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Get all components and if they are not defined in parameters activate them automatically auto components_to_activate = resource_manager_->get_components_status(); @@ -374,6 +651,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; auto set_components_to_state = +<<<<<<< HEAD [&](const std::string & parameter_name, rclcpp_lifecycle::State state) { std::vector components_to_set = std::vector({}); @@ -406,17 +684,59 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } components_to_activate.erase(component); } +======= + [&](const std::vector & components_to_set, rclcpp_lifecycle::State state) + { + for (const auto & component : components_to_set) + { + if (component.empty()) + { + continue; + } + if (components_to_activate.find(component) == components_to_activate.end()) + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + if ( + resource_manager_->set_component_state(component, state) == + hardware_interface::return_type::ERROR) + { + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + state.label()); + } + components_to_activate.erase(component); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } }; +<<<<<<< HEAD // unconfigured (loaded only) set_components_to_state( "hardware_components_initial_state.unconfigured", +======= + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + // unconfigured (loaded only) + set_components_to_state( + params_->hardware_components_initial_state.unconfigured, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclcpp_lifecycle::State( State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) +<<<<<<< HEAD // BEGIN: Keep old functionality on for backwards compatibility std::vector configure_components_on_start = std::vector({}); get_parameter("configure_components_on_start", configure_components_on_start); @@ -475,6 +795,28 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } } } +======= + set_components_to_state( + params_->hardware_components_initial_state.inactive, + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); + + // activate all other components + for (const auto & [component, state] : components_to_activate) + { + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + if ( + resource_manager_->set_component_state(component, active_state) == + hardware_interface::return_type::ERROR) + { + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + active_state.label()); + } + } + robot_description_notification_timer_->cancel(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void ControllerManager::init_services() @@ -488,6 +830,7 @@ void ControllerManager::init_services() using namespace std::placeholders; list_controllers_service_ = create_service( "~/list_controllers", std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2), +<<<<<<< HEAD rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); list_controller_types_service_ = create_service( @@ -501,10 +844,26 @@ void ControllerManager::init_services() "~/configure_controller", std::bind(&ControllerManager::configure_controller_service_cb, this, _1, _2), rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); +======= + qos_services, best_effort_callback_group_); + list_controller_types_service_ = + create_service( + "~/list_controller_types", + std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2), qos_services, + best_effort_callback_group_); + load_controller_service_ = create_service( + "~/load_controller", std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2), + qos_services, best_effort_callback_group_); + configure_controller_service_ = create_service( + "~/configure_controller", + std::bind(&ControllerManager::configure_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) reload_controller_libraries_service_ = create_service( "~/reload_controller_libraries", std::bind(&ControllerManager::reload_controller_libraries_service_cb, this, _1, _2), +<<<<<<< HEAD rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); switch_controller_service_ = create_service( "~/switch_controller", @@ -524,11 +883,36 @@ void ControllerManager::init_services() "~/list_hardware_interfaces", std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); +======= + qos_services, best_effort_callback_group_); + switch_controller_service_ = create_service( + "~/switch_controller", + std::bind(&ControllerManager::switch_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); + unload_controller_service_ = create_service( + "~/unload_controller", + std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); + list_hardware_components_service_ = + create_service( + "~/list_hardware_components", + std::bind(&ControllerManager::list_hardware_components_srv_cb, this, _1, _2), qos_services, + best_effort_callback_group_); + list_hardware_interfaces_service_ = + create_service( + "~/list_hardware_interfaces", + std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), qos_services, + best_effort_callback_group_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) set_hardware_component_state_service_ = create_service( "~/set_hardware_component_state", std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), +<<<<<<< HEAD rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); +======= + qos_services, best_effort_callback_group_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( @@ -572,8 +956,14 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c catch (const std::exception & e) { RCLCPP_ERROR( +<<<<<<< HEAD get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s", controller_name.c_str(), controller_type.c_str(), e.what()); +======= + get_logger(), + "Caught exception of type : %s while loading the controller '%s' of plugin type '%s':\n%s", + typeid(e).name(), controller_name.c_str(), controller_type.c_str(), e.what()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return nullptr; } catch (...) @@ -589,6 +979,13 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; +<<<<<<< HEAD +======= + controller_spec.last_update_cycle_time = std::make_shared( + 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + controller_spec.execution_time_statistics = std::make_shared(); + controller_spec.periodicity_statistics = std::make_shared(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // We have to fetch the parameters_file at the time of loading the controller, because this way we // can load them at the creation of the LifeCycleNode and this helps in using the features such as @@ -619,6 +1016,29 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } } +<<<<<<< HEAD +======= + const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; + std::vector fallback_controllers; + if (!has_parameter(fallback_ctrl_param)) + { + declare_parameter(fallback_ctrl_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) + { + if ( + std::find(fallback_controllers.begin(), fallback_controllers.end(), controller_name) != + fallback_controllers.end()) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' cannot be a fallback controller for itself.", + controller_name.c_str()); + return nullptr; + } + controller_spec.info.fallback_controllers_names = fallback_controllers; + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return add_controller_impl(controller_spec); } @@ -644,12 +1064,22 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c get_logger(), "The 'type' param was not defined for '%s'.", controller_name.c_str()); return nullptr; } +<<<<<<< HEAD +======= + RCLCPP_INFO( + get_logger(), "Loading controller : '%s' of type '%s'", controller_name.c_str(), + controller_type.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return load_controller(controller_name, controller_type); } controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { +<<<<<<< HEAD +======= + RCLCPP_INFO(get_logger(), "Unloading controller: '%s'", controller_name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); @@ -683,6 +1113,10 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); +<<<<<<< HEAD +======= + controller_chain_spec_cleanup(controller_chain_spec_, controller_name); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? if (is_controller_inactive(*controller.c)) @@ -691,11 +1125,35 @@ controller_interface::return_type ControllerManager::unload_controller( get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? +<<<<<<< HEAD const auto new_state = controller.c->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_WARN( get_logger(), "Failed to clean-up the controller '%s' before unloading!", +======= + try + { + const auto new_state = controller.c->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_WARN( + get_logger(), "Failed to clean-up the controller '%s' before unloading!", + controller_name.c_str()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), + "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s", + typeid(e).name(), controller_name.c_str(), e.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Failed to clean-up the controller '%s' before unloading", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_name.c_str()); } } @@ -711,6 +1169,10 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_interface::return_type::OK; } @@ -723,7 +1185,11 @@ std::vector ControllerManager::get_loaded_controllers() const controller_interface::return_type ControllerManager::configure_controller( const std::string & controller_name) { +<<<<<<< HEAD RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str()); +======= + RCLCPP_INFO(get_logger(), "Configuring controller: '%s'", controller_name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto & controllers = get_loaded_controllers(); @@ -741,7 +1207,11 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; +<<<<<<< HEAD auto state = controller->get_state(); +======= + auto state = controller->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -752,29 +1222,77 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } +<<<<<<< HEAD auto new_state = controller->get_state(); +======= + auto new_state = controller->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? +<<<<<<< HEAD new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_ERROR( get_logger(), "Controller '%s' can not be cleaned-up before configuring", +======= + try + { + new_state = controller->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' can not be cleaned-up before configuring", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught exception while cleaning-up controller '%s' before configuring", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_name.c_str()); return controller_interface::return_type::ERROR; } } +<<<<<<< HEAD new_state = controller->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", controller_name.c_str(), new_state.label().c_str()); +======= + try + { + new_state = controller->configure(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", + controller_name.c_str(), new_state.label().c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while configuring controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); + return controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while configuring controller '%s'", + controller_name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_interface::return_type::ERROR; } @@ -789,6 +1307,7 @@ controller_interface::return_type ControllerManager::configure_controller( "update rate.", controller_name.c_str(), controller_update_rate, cm_update_rate); } +<<<<<<< HEAD else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0) { // NOTE: The following computation is done to compute the approx controller update that can be @@ -801,6 +1320,17 @@ controller_interface::return_type ControllerManager::configure_controller( "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's " "update rate which is : %d Hz.", controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate); +======= + else if (cm_update_rate % controller_update_rate != 0) + { + RCLCPP_WARN( + get_logger(), + "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the " + "controller's update rate : %d Hz is not a perfect divisor of the controller manager's " + "update rate : %d Hz!.", + controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate, + cm_update_rate); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -810,6 +1340,7 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); +<<<<<<< HEAD auto interfaces = controller->export_reference_interfaces(); if (interfaces.empty()) { @@ -820,6 +1351,66 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); +======= + std::vector state_interfaces; + std::vector ref_interfaces; + try + { + state_interfaces = controller->export_state_interfaces(); + ref_interfaces = controller->export_reference_interfaces(); + if (ref_interfaces.empty() && state_interfaces.empty()) + { + // TODO(destogl): Add test for this! + RCLCPP_ERROR( + get_logger(), + "Controller '%s' is chainable, but does not export any state or reference interfaces. " + "Did you override the on_export_method() correctly?", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::exception & e) + { + RCLCPP_FATAL( + get_logger(), "Export of the state or reference interfaces failed with following error: %s", + e.what()); + return controller_interface::return_type::ERROR; + } + resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); + resource_manager_->import_controller_exported_state_interfaces( + controller_name, state_interfaces); + } + + // let's update the list of following and preceding controllers + const auto cmd_itfs = controller->command_interface_configuration().names; + const auto state_itfs = controller->state_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) + { + add_element_to_list( + controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); + add_element_to_list( + controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name); + add_element_to_list( + controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller_name); + } + } + // This is needed when we start exporting the state interfaces from the controllers + for (const auto & state_itf : state_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) + { + add_element_to_list( + controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); + add_element_to_list( + controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name); + add_element_to_list( + controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller_name); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // Now let's reorder the controllers @@ -830,6 +1421,7 @@ controller_interface::return_type ControllerManager::configure_controller( // Copy all controllers from the 'from' list to the 'to' list to = from; +<<<<<<< HEAD // Reordering the controllers std::stable_sort( @@ -838,6 +1430,34 @@ controller_interface::return_type ControllerManager::configure_controller( &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, to)); +======= + std::vector sorted_list; + + // clear the list before reordering it again + ordered_controllers_names_.clear(); + for (const auto & [ctrl_name, chain_spec] : controller_chain_spec_) + { + auto it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + if (it == ordered_controllers_names_.end()) + { + update_list_with_controller_chain(ctrl_name, ordered_controllers_names_.end(), false); + } + } + + std::vector new_list; + for (const auto & ctrl : ordered_controllers_names_) + { + auto controller_it = std::find_if( + to.begin(), to.end(), std::bind(controller_name_compare, std::placeholders::_1, ctrl)); + if (controller_it != to.end()) + { + new_list.push_back(*controller_it); + } + } + + to = new_list; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); for (const auto & ctrl : to) { @@ -861,6 +1481,10 @@ void ControllerManager::clear_requests() // state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { +<<<<<<< HEAD +======= + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); @@ -874,6 +1498,18 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { +<<<<<<< HEAD +======= + if (!is_resource_manager_initialized()) + { + RCLCPP_ERROR( + get_logger(), + "Resource Manager is not initialized yet! Please provide robot description on " + "'robot_description' topic before trying to switch controllers."); + return controller_interface::return_type::ERROR; + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // reset the switch param internal variables switch_params_.reset(); @@ -914,6 +1550,7 @@ controller_interface::return_type ControllerManager::switch_controller( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } +<<<<<<< HEAD RCLCPP_DEBUG(get_logger(), "Switching controllers:"); for (const auto & controller : activate_controllers) { @@ -923,6 +1560,26 @@ controller_interface::return_type ControllerManager::switch_controller( { RCLCPP_DEBUG(get_logger(), "- Deactivating controller '%s'", controller.c_str()); } +======= + std::string activate_list, deactivate_list; + activate_list.reserve(500); + deactivate_list.reserve(500); + for (const auto & controller : activate_controllers) + { + activate_list.append(controller); + activate_list.append(" "); + } + for (const auto & controller : deactivate_controllers) + { + deactivate_list.append(controller); + deactivate_list.append(" "); + } + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", + deactivate_list.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto list_controllers = [this, strictness]( const std::vector & controller_list, @@ -1016,6 +1673,14 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } +<<<<<<< HEAD +======= + if (status == controller_interface::return_type::OK) + { + status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (status != controller_interface::return_type::OK) { RCLCPP_WARN( @@ -1090,6 +1755,17 @@ controller_interface::return_type ControllerManager::switch_controller( } } +<<<<<<< HEAD +======= + // Check after the check if the activate and deactivate list is empty or not + if (activate_request_.empty() && deactivate_request_.empty()) + { + RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); + clear_requests(); + return controller_interface::return_type::OK; + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & controller : controllers) { auto to_chained_mode_list_it = std::find( @@ -1211,15 +1887,83 @@ controller_interface::return_type ControllerManager::switch_controller( { extract_interfaces_for_controller(controller, deactivate_command_interface_request_); } - } - - if (activate_request_.empty() && deactivate_request_.empty()) - { - RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); - clear_requests(); - return controller_interface::return_type::OK; - } +<<<<<<< HEAD +======= + + // cache mapping between hardware and controllers for stopping when read/write error happens + // TODO(destogl): This caching approach is suboptimal because the cache can fast become + // outdated. Keeping it up to date is not easy because of stopping controllers from multiple + // threads maybe we should not at all cache this but always search for the related controllers + // to a hardware when error in hardware happens + if (in_activate_list) + { + std::vector interface_names = {}; + auto command_interface_config = controller.c->command_interface_configuration(); + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + interface_names = resource_manager_->available_command_interfaces(); + } + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + interface_names = command_interface_config.names; + } + + std::vector interfaces = {}; + auto state_interface_config = controller.c->state_interface_configuration(); + if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + interfaces = resource_manager_->available_state_interfaces(); + } + if ( + state_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + interfaces = state_interface_config.names; + } + + interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end()); + + resource_manager_->cache_controller_to_hardware(controller.info.name, interface_names); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + } + + if (activate_request_.empty() && deactivate_request_.empty()) + { + RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); + clear_requests(); + return controller_interface::return_type::OK; + } + +<<<<<<< HEAD +======= + RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); + for (const auto & interface : activate_command_interface_request_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + RCLCPP_DEBUG(get_logger(), "Release of command interfaces from deactivating controllers:"); + for (const auto & interface : deactivate_command_interface_request_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + + // wait for deactivating async controllers to finish their current cycle + for (const auto & controller : deactivate_request_) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller)); + if (controller_it != controllers.end()) + { + controller_it->c->wait_for_trigger_update_to_finish(); + } + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if ( !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) { @@ -1233,6 +1977,10 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } } +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; @@ -1323,6 +2071,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller); +<<<<<<< HEAD if ( controller.c->init(controller.info.name, get_namespace(), controller_node_options) == controller_interface::return_type::ERROR) @@ -1332,6 +2081,45 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co get_logger(), "Could not initialize the controller named '%s'", controller.info.name.c_str()); return nullptr; } +======= + // Catch whatever exception the controller might throw + try + { + if ( + controller.c->init( + controller.info.name, robot_description_, get_update_rate(), get_namespace(), + controller_node_options) == controller_interface::return_type::ERROR) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Could not initialize the controller named '%s'", + controller.info.name.c_str()); + return nullptr; + } + } + catch (const std::exception & e) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while initializing controller '%s': %s", + typeid(e).name(), controller.info.name.c_str(), e.what()); + return nullptr; + } + catch (...) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while initializing controller '%s'", + controller.info.name.c_str()); + return nullptr; + } + + // initialize the data for the controller chain spec once it is loaded. It is needed, so when we + // sort the controllers later, they will be added to the list + controller_chain_spec_[controller.info.name] = ControllerChainSpec(); + controller_chained_state_interfaces_cache_[controller.info.name] = {}; + controller_chained_reference_interfaces_cache_[controller.info.name] = {}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); @@ -1347,6 +2135,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return to.back().c; } +<<<<<<< HEAD void ControllerManager::manage_switch() { std::unique_lock guard(switch_params_.mutex, std::try_to_lock); @@ -1390,6 +2179,14 @@ void ControllerManager::deactivate_controllers() rt_controllers_wrapper_.update_and_get_used_by_rt_list(); // stop controllers for (const auto & controller_name : deactivate_request_) +======= +void ControllerManager::deactivate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_deactivate) +{ + // deactivate controllers + for (const auto & controller_name : controllers_to_deactivate) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1398,13 +2195,18 @@ void ControllerManager::deactivate_controllers() { RCLCPP_ERROR( get_logger(), +<<<<<<< HEAD "Got request to stop controller '%s' but it is not in the realtime controller list", +======= + "Got request to deactivate controller '%s' but it is not in the realtime controller list", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_name.c_str()); continue; } auto controller = found_it->c; if (is_controller_active(*controller)) { +<<<<<<< HEAD const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -1412,6 +2214,40 @@ void ControllerManager::deactivate_controllers() RCLCPP_ERROR( get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", controller_name.c_str(), new_state.label().c_str()); +======= + try + { + const auto new_state = controller->get_node()->deactivate(); + controller->release_interfaces(); + + // if it is a chainable controller, make the reference interfaces unavailable on + // deactivation + if (controller->is_chainable()) + { + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", + controller_name.c_str(), new_state.label().c_str()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while deactivating the controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); + continue; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while deactivating the controller '%s'", + controller_name.c_str()); + continue; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } } @@ -1440,6 +2276,7 @@ void ControllerManager::switch_chained_mode( auto controller = found_it->c; if (!is_controller_active(*controller)) { +<<<<<<< HEAD if (controller->set_chained_mode(to_chained_mode)) { if (to_chained_mode) @@ -1452,6 +2289,9 @@ void ControllerManager::switch_chained_mode( } } else if (!controller->set_chained_mode(to_chained_mode)) +======= + if (!controller->set_chained_mode(to_chained_mode)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { RCLCPP_ERROR( get_logger(), @@ -1474,11 +2314,19 @@ void ControllerManager::switch_chained_mode( } } +<<<<<<< HEAD void ControllerManager::activate_controllers() { std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); for (const auto & controller_name : activate_request_) +======= +void ControllerManager::activate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate) +{ + for (const auto & controller_name : controllers_to_activate) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1492,6 +2340,12 @@ void ControllerManager::activate_controllers() continue; } auto controller = found_it->c; +<<<<<<< HEAD +======= + // 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()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) bool assignment_successful = true; // assign command interfaces to the controller @@ -1528,7 +2382,14 @@ void ControllerManager::activate_controllers() catch (const std::exception & e) { RCLCPP_ERROR( +<<<<<<< HEAD get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what()); +======= + get_logger(), + "Caught exception of type : %s while claiming the command interfaces. Can't activate " + "controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) assignment_successful = false; break; } @@ -1563,7 +2424,14 @@ void ControllerManager::activate_controllers() catch (const std::exception & e) { RCLCPP_ERROR( +<<<<<<< HEAD get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what()); +======= + get_logger(), + "Caught exception of type : %s while claiming the state interfaces. Can't activate " + "controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) assignment_successful = false; break; } @@ -1575,6 +2443,7 @@ void ControllerManager::activate_controllers() } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); +<<<<<<< HEAD const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -1584,11 +2453,42 @@ void ControllerManager::activate_controllers() controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), hardware_interface::lifecycle_state_names::ACTIVE, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + try + { + found_it->periodicity_statistics->Reset(); + found_it->execution_time_statistics->Reset(); + const auto new_state = controller->get_node()->activate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR( + get_logger(), + "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", + controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), + hardware_interface::lifecycle_state_names::ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); + continue; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while activating the controller '%s'", + controller_name.c_str()); + continue; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // if it is a chainable controller, make the reference interfaces available on activation if (controller->is_chainable()) { +<<<<<<< HEAD resource_manager_->make_controller_reference_interfaces_available(controller_name); } } @@ -1600,6 +2500,21 @@ void ControllerManager::activate_controllers_asap() { // https://github.com/ros-controls/ros2_control/issues/263 activate_controllers(); +======= + // make all the exported interfaces of the controller available + resource_manager_->make_controller_exported_state_interfaces_available(controller_name); + resource_manager_->make_controller_reference_interfaces_available(controller_name); + } + } +} + +void ControllerManager::activate_controllers_asap( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate) +{ + // https://github.com/ros-controls/ros2_control/issues/263 + activate_controllers(rt_controller_list, controllers_to_activate); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void ControllerManager::list_controllers_srv_cb( @@ -1631,7 +2546,11 @@ void ControllerManager::list_controllers_srv_cb( controller_state.name = controllers[i].info.name; controller_state.type = controllers[i].info.type; controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces; +<<<<<<< HEAD controller_state.state = controllers[i].c->get_state().label(); +======= + controller_state.state = controllers[i].c->get_lifecycle_state().label(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_state.is_chainable = controllers[i].c->is_chainable(); controller_state.is_chained = controllers[i].c->is_in_chained_mode(); @@ -1678,13 +2597,30 @@ void ControllerManager::list_controllers_srv_cb( { auto references = resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); +<<<<<<< HEAD controller_state.reference_interfaces.reserve(references.size()); +======= + auto exported_state_interfaces = + resource_manager_->get_controller_exported_state_interface_names( + controllers[i].info.name); + controller_state.reference_interfaces.reserve(references.size()); + controller_state.exported_state_interfaces.reserve(exported_state_interfaces.size()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & reference : references) { const std::string prefix_name = controllers[i].c->get_node()->get_name(); const std::string interface_name = reference.substr(prefix_name.size() + 1); controller_state.reference_interfaces.push_back(interface_name); } +<<<<<<< HEAD +======= + for (const auto & state_interface : exported_state_interfaces) + { + const std::string prefix_name = controllers[i].c->get_node()->get_name(); + const std::string interface_name = state_interface.substr(prefix_name.size() + 1); + controller_state.exported_state_interfaces.push_back(interface_name); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } response->controller.push_back(controller_state); @@ -1865,6 +2801,7 @@ void ControllerManager::switch_controller_service_cb( std::lock_guard guard(services_lock_); RCLCPP_DEBUG(get_logger(), "switching service locked"); +<<<<<<< HEAD // response->ok = switch_controller( // request->activate_controllers, request->deactivate_controllers, request->strictness, // request->activate_asap, request->timeout) == controller_interface::return_type::OK; @@ -1905,6 +2842,12 @@ void ControllerManager::switch_controller_service_cb( activate_controllers, deactivate_controllers, request->strictness, activate_asap, request->timeout) == controller_interface::return_type::OK; // END: remove when deprecated removed +======= + response->ok = + switch_controller( + request->activate_controllers, request->deactivate_controllers, request->strictness, + request->activate_asap, request->timeout) == controller_interface::return_type::OK; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) RCLCPP_DEBUG(get_logger(), "switching service finished"); } @@ -1942,7 +2885,11 @@ void ControllerManager::list_hardware_components_srv_cb( auto component = controller_manager_msgs::msg::HardwareComponentState(); component.name = component_name; component.type = component_info.type; +<<<<<<< HEAD component.class_type = component_info.class_type; +======= + component.plugin_name = component_info.plugin_name; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) component.state.id = component_info.state.id(); component.state.label = component_info.state.label(); @@ -1953,6 +2900,23 @@ void ControllerManager::list_hardware_components_srv_cb( hwi.name = interface; hwi.is_available = resource_manager_->command_interface_is_available(interface); hwi.is_claimed = resource_manager_->command_interface_is_claimed(interface); +<<<<<<< HEAD +======= + // TODO(destogl): Add here mapping to controller that has claimed or + // can be claiming this interface + // Those should be two variables + // if (hwi.is_claimed) + // { + // for (const auto & controller : controllers_that_use_interface(interface)) + // { + // if (is_controller_active(controller)) + // { + // hwi.is_claimed_by = controller; + // } + // } + // } + // hwi.is_used_by = controllers_that_use_interface(interface); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) component.command_interfaces.push_back(hwi); } @@ -2051,7 +3015,86 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { +<<<<<<< HEAD resource_manager_->read(time, period); +======= + periodicity_stats_.AddMeasurement(1.0 / period.seconds()); + auto [ok, failed_hardware_names] = resource_manager_->read(time, period); + + if (!ok) + { + std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); + // Determine controllers to stop + for (const auto & hardware_name : failed_hardware_names) + { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); + } + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their read cycle resulted in an error: [ %s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components read cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + deactivate_controllers(rt_controller_list, stop_request); + // TODO(destogl): do auto-start of broadcasters + } +} + +void ControllerManager::manage_switch() +{ + std::unique_lock guard(switch_params_.mutex, std::try_to_lock); + if (!guard.owns_lock()) + { + RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); + return; + } + // Ask hardware interfaces to change mode + if (!resource_manager_->perform_command_mode_switch( + activate_command_interface_request_, deactivate_command_interface_request_)) + { + RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); + } + + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + deactivate_controllers(rt_controller_list, deactivate_request_); + + switch_chained_mode(to_chained_mode_request_, true); + switch_chained_mode(from_chained_mode_request_, false); + + // activate controllers once the switch is fully complete + if (!switch_params_.activate_asap) + { + activate_controllers(rt_controller_list, activate_request_); + } + else + { + // activate controllers as soon as their required joints are done switching + activate_controllers_asap(rt_controller_list, activate_request_); + } + + // All controllers switched --> switching done + switch_params_.do_switch = false; + switch_params_.cv.notify_all(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } controller_interface::return_type ControllerManager::update( @@ -2064,12 +3107,37 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; +<<<<<<< HEAD for (auto loaded_controller : rt_controller_list) +======= + // Check for valid time + if (!get_clock()->started()) + { + if (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + throw std::runtime_error( + "No clock received, and time argument is zero. Check your controller_manager node's " + "clock configuration (use_sim_time parameter) and if a valid clock source is " + "available. Also pass a proper time argument to the update method."); + } + + // this can happen with use_sim_time=true until the /clock is received + rclcpp::Clock clock = rclcpp::Clock(); + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 1000, + "No clock received, using time argument instead! Check your node's clock " + "configuration (use_sim_time parameter) and if a valid clock source is available"); + } + + std::vector failed_controllers_list; + for (const auto & loaded_controller : rt_controller_list) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 if (is_controller_active(*loaded_controller.c)) { +<<<<<<< HEAD const auto controller_update_rate = loaded_controller.c->get_update_rate(); const auto controller_update_factor = (controller_update_rate == 0) || (controller_update_rate >= update_rate_) @@ -2077,6 +3145,53 @@ controller_interface::return_type ControllerManager::update( : update_rate_ / controller_update_rate; bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0); +======= + if ( + switch_params_.do_switch && loaded_controller.c->is_async() && + std::find( + deactivate_request_.begin(), deactivate_request_.end(), loaded_controller.info.name) != + deactivate_request_.end()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping update for async controller '%s' as it is being deactivated", + loaded_controller.info.name.c_str()); + continue; + } + const auto controller_update_rate = loaded_controller.c->get_update_rate(); + const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_); + const auto controller_period = + run_controller_at_cm_rate ? period + : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + + bool first_update_cycle = false; + const rclcpp::Time current_time = get_clock()->started() ? get_clock()->now() : time; + if ( + *loaded_controller.last_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + // last_update_cycle_time is zero after activation + first_update_cycle = true; + *loaded_controller.last_update_cycle_time = current_time; + RCLCPP_DEBUG( + 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()); + } + 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())) || + (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", @@ -2084,6 +3199,7 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { +<<<<<<< HEAD auto controller_ret = loaded_controller.c->update( time, (controller_update_factor != 1u) ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) @@ -2091,11 +3207,121 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { +======= + auto controller_ret = controller_interface::return_type::OK; + bool trigger_status = true; + // Catch exceptions thrown by the controller update function + try + { + const auto trigger_result = + loaded_controller.c->trigger_update(current_time, controller_actual_period); + trigger_status = trigger_result.successful; + controller_ret = trigger_result.result; + if (trigger_status && trigger_result.execution_time.has_value()) + { + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (!first_update_cycle && trigger_status && trigger_result.period.has_value()) + { + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / trigger_result.period.value().seconds()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while updating controller '%s': %s", + typeid(e).name(), loaded_controller.info.name.c_str(), e.what()); + controller_ret = controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while updating controller '%s'", + loaded_controller.info.name.c_str()); + controller_ret = controller_interface::return_type::ERROR; + } + + *loaded_controller.last_update_cycle_time = current_time; + + if (controller_ret != controller_interface::return_type::OK) + { + failed_controllers_list.push_back(loaded_controller.info.name); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ret = controller_ret; } } } } +<<<<<<< HEAD +======= + if (!failed_controllers_list.empty()) + { + const auto FALLBACK_STACK_MAX_SIZE = 500; + std::vector active_controllers_using_interfaces(failed_controllers_list); + active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE); + std::vector cumulative_fallback_controllers; + cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE); + + for (const auto & failed_ctrl : failed_controllers_list) + { + auto ctrl_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, failed_ctrl)); + if (ctrl_it != rt_controller_list.end()) + { + for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names) + { + cumulative_fallback_controllers.push_back(fallback_controller); + get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list, active_controllers_using_interfaces); + } + } + } + std::string controllers_string; + controllers_string.reserve(500); + for (const auto & controller : failed_controllers_list) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!", + controllers_string.c_str()); + if (active_controllers_using_interfaces.size() > failed_controllers_list.size()) + { + controllers_string.clear(); + for (size_t i = failed_controllers_list.size(); + i < active_controllers_using_interfaces.size(); i++) + { + controllers_string.append(active_controllers_using_interfaces[i]); + controllers_string.append(" "); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !controllers_string.empty(), + "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " + "controllers to activate.", + controllers_string.c_str()); + } + if (!cumulative_fallback_controllers.empty()) + { + controllers_string.clear(); + for (const auto & controller : cumulative_fallback_controllers) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); + } + deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); + if (!cumulative_fallback_controllers.empty()) + { + activate_controllers(rt_controller_list, cumulative_fallback_controllers); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // there are controllers to (de)activate if (switch_params_.do_switch) @@ -2108,7 +3334,47 @@ controller_interface::return_type ControllerManager::update( void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) { +<<<<<<< HEAD resource_manager_->write(time, period); +======= + auto [ok, failed_hardware_names] = resource_manager_->write(time, period); + + if (!ok) + { + std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); + // Determine controllers to stop + for (const auto & hardware_name : failed_hardware_names) + { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); + } + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their write cycle resulted in an error: [ " + "%s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components write cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + deactivate_controllers(rt_controller_list, stop_request); + // TODO(destogl): do auto-start of broadcasters + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } std::vector & @@ -2187,6 +3453,14 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +<<<<<<< HEAD +======= +void ControllerManager::shutdown_async_controllers_and_components() +{ + resource_manager_->shutdown_async_components(); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { @@ -2211,12 +3485,25 @@ void ControllerManager::propagate_deactivation_of_chained_mode( break; } +<<<<<<< HEAD for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) { // controller that 'cmd_tf_name' belongs to ControllersListIterator following_ctrl_it; if (command_interface_is_reference_interface_of_controller( cmd_itf_name, controllers, following_ctrl_it)) +======= + const auto ctrl_cmd_itf_names = controller.c->command_interface_configuration().names; + const auto ctrl_state_itf_names = controller.c->state_interface_configuration().names; + auto ctrl_itf_names = ctrl_cmd_itf_names; + ctrl_itf_names.insert( + ctrl_itf_names.end(), ctrl_state_itf_names.begin(), ctrl_state_itf_names.end()); + for (const auto & ctrl_itf_name : ctrl_itf_names) + { + // controller that 'cmd_tf_name' belongs to + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // currently iterated "controller" is preceding controller --> add following controller // with matching interface name to "from" chained mode list (if not already in it) @@ -2245,12 +3532,30 @@ controller_interface::return_type ControllerManager::check_following_controllers get_logger(), "Checking following controllers of preceding controller with name '%s'.", controller_it->info.name.c_str()); +<<<<<<< HEAD for (const auto & cmd_itf_name : controller_it->c->command_interface_configuration().names) { ControllersListIterator following_ctrl_it; // Check if interface if reference interface and following controller exist. if (!command_interface_is_reference_interface_of_controller( cmd_itf_name, controllers, following_ctrl_it)) +======= + const auto controller_cmd_interfaces = controller_it->c->command_interface_configuration().names; + const auto controller_state_interfaces = controller_it->c->state_interface_configuration().names; + // get all interfaces of the controller + auto controller_interfaces = controller_cmd_interfaces; + controller_interfaces.insert( + controller_interfaces.end(), controller_state_interfaces.begin(), + controller_state_interfaces.end()); + for (const auto & ctrl_itf_name : controller_interfaces) + { + RCLCPP_DEBUG( + get_logger(), "Checking interface '%s' of controller '%s'.", ctrl_itf_name.c_str(), + controller_it->info.name.c_str()); + ControllersListIterator following_ctrl_it; + // Check if interface if reference interface and following controller exist. + if (!is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { continue; } @@ -2270,9 +3575,15 @@ controller_interface::return_type ControllerManager::check_following_controllers { RCLCPP_WARN( get_logger(), +<<<<<<< HEAD "No reference interface '%s' exist, since the following controller with name '%s' " "is not chainable.", cmd_itf_name.c_str(), following_ctrl_it->info.name.c_str()); +======= + "No state/reference interface '%s' exist, since the following controller with name " + "'%s' is not chainable.", + ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_interface::return_type::ERROR; } @@ -2325,6 +3636,7 @@ controller_interface::return_type ControllerManager::check_following_controllers following_ctrl_it->info.name); if (found_it == to_chained_mode_request_.end()) { +<<<<<<< HEAD to_chained_mode_request_.push_back(following_ctrl_it->info.name); // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) @@ -2333,6 +3645,25 @@ controller_interface::return_type ControllerManager::check_following_controllers RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'to chained mode' request.", following_ctrl_it->info.name.c_str()); +======= + // if it is a chainable controller, make the reference interfaces available on preactivation + // (This is needed when you activate a couple of chainable controller altogether) + // make all the exported interfaces of the controller available + resource_manager_->make_controller_exported_state_interfaces_available( + following_ctrl_it->info.name); + if ( + std::find( + controller_cmd_interfaces.begin(), controller_cmd_interfaces.end(), ctrl_itf_name) != + controller_cmd_interfaces.end()) + { + resource_manager_->make_controller_reference_interfaces_available( + following_ctrl_it->info.name); + to_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'to chained mode' request.", + following_ctrl_it->info.name.c_str()); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } else @@ -2365,6 +3696,7 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +<<<<<<< HEAD if (!controller_it->c->is_in_chained_mode()) { RCLCPP_DEBUG( @@ -2375,10 +3707,13 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) RCLCPP_DEBUG( get_logger(), "Checking preceding controller of following controller with name '%s'.", controller_it->info.name.c_str()); +<<<<<<< HEAD for (const auto & ref_itf_name : resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) { @@ -2405,11 +3740,33 @@ controller_interface::return_type ControllerManager::check_preceeding_controller is_controller_inactive(preceding_ctrl_it->c) && std::find( activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != +======= + auto preceeding_controllers_list = + controller_chained_state_interfaces_cache_[controller_it->info.name]; + preceeding_controllers_list.insert( + preceeding_controllers_list.end(), + controller_chained_reference_interfaces_cache_[controller_it->info.name].cbegin(), + controller_chained_reference_interfaces_cache_[controller_it->info.name].cend()); + + for (const auto & preceeding_controller : preceeding_controllers_list) + { + RCLCPP_DEBUG(get_logger(), "\t Preceding controller : '%s'.", preceeding_controller.c_str()); + auto found_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, preceeding_controller)); + + if (found_it != controllers.end()) + { + if ( + is_controller_inactive(found_it->c) && + std::find(activate_request_.begin(), activate_request_.end(), preceeding_controller) != +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) activate_request_.end()) { RCLCPP_WARN( get_logger(), "Could not deactivate controller with name '%s' because " +<<<<<<< HEAD "preceding controller with name '%s' will be activated. ", controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; @@ -2419,12 +3776,22 @@ controller_interface::return_type ControllerManager::check_preceeding_controller is_controller_active(preceding_ctrl_it->c) && std::find( deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == +======= + "preceding controller with name '%s' is inactive and will be activated.", + controller_it->info.name.c_str(), preceeding_controller.c_str()); + return controller_interface::return_type::ERROR; + } + if ( + is_controller_active(found_it->c) && + std::find(deactivate_request_.begin(), deactivate_request_.end(), preceeding_controller) == +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) deactivate_request_.end()) { RCLCPP_WARN( get_logger(), "Could not deactivate controller with name '%s' because " "preceding controller with name '%s' is active and will not be deactivated.", +<<<<<<< HEAD controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); return controller_interface::return_type::ERROR; } @@ -2436,11 +3803,184 @@ controller_interface::return_type ControllerManager::check_preceeding_controller // // insert to the begin of activate request list to be activated before preceding controller // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); // } +======= + controller_it->info.name.c_str(), preceeding_controller.c_str()); + return controller_interface::return_type::ERROR; + } } } + + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if ( + // strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of activate request list to be activated before preceding + // controller + // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); + // } + return controller_interface::return_type::OK; } +controller_interface::return_type +ControllerManager::check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it) +{ + for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) + { + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) + { + RCLCPP_ERROR( + get_logger(), + "Unable to find the fallback controller : '%s' of the controller : '%s' " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" + " need to be configured and be in inactive/active state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + for (const auto & fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) + { + if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_ref_itfs = + resource_manager_->get_controller_reference_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == + exported_ref_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + for (const auto & fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) + { + if (!resource_manager_->state_interface_is_available(fb_state_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_state_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_state_itfs = + resource_manager_->get_controller_exported_state_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == + exported_state_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + } + } + return controller_interface::return_type::OK; +} + +<<<<<<< HEAD bool ControllerManager::controller_sorting( const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers) @@ -2565,6 +4105,333 @@ bool ControllerManager::controller_sorting( return false; } }; +======= +void ControllerManager::controller_activity_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + atleast_one_hw_active = true; + break; + } + } + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + bool all_active = true; + const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + const std::string state_suffix = ".state"; + + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + auto make_stats_string = + [](const auto & statistics_data, const std::string & measurement_unit) -> std::string + { + std::ostringstream oss; + oss << std::fixed << std::setprecision(2); + oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " + << statistics_data.max << "] " << measurement_unit + << ", StdDev: " << statistics_data.standard_deviation; + return oss.str(); + }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_controllers; + std::vector bad_periodicity_async_controllers; + for (size_t i = 0; i < controllers.size(); ++i) + { + const bool is_async = controllers[i].c->is_async(); + if (!is_controller_active(controllers[i].c)) + { + all_active = false; + } + stat.add( + controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); + if (is_controller_active(controllers[i].c)) + { + const auto periodicity_stats = controllers[i].periodicity_statistics->GetStatistics(); + const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics(); + stat.add( + controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(controllers[i].c->get_update_rate()) + " Hz"); + const double periodicity_error = std::abs( + periodicity_stats.average - static_cast(controllers[i].c->get_update_rate())); + if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.error || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + } + const double max_exp_exec_time = is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.error || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_controllers.push_back(controllers[i].info.name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_controllers.push_back(controllers[i].info.name); + } + } + } + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + + if (!high_exec_time_controllers.empty()) + { + std::string high_exec_time_controllers_string; + for (const auto & controller : high_exec_time_controllers) + { + high_exec_time_controllers_string.append(controller); + high_exec_time_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); + } + if (!bad_periodicity_async_controllers.empty()) + { + std::string bad_periodicity_async_controllers_string; + for (const auto & controller : bad_periodicity_async_controllers) + { + bad_periodicity_async_controllers_string.append(controller); + bad_periodicity_async_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with bad periodicity : [ " + bad_periodicity_async_controllers_string + "]"); + } + + if (!atleast_one_hw_active) + { + stat.mergeSummary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "No hardware components are currently active to activate controllers"); + } + else if (controllers.empty()) + { + stat.mergeSummary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); + } +} + +void ControllerManager::hardware_components_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool all_active = true; + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + stat.add(component_name, component_info.state.label()); + if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + all_active = false; + } + else + { + atleast_one_hw_active = true; + } + } + if (!is_resource_manager_initialized()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + } + else if (hw_components_info.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + } + else + { + if (!atleast_one_hw_active) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + "No hardware components are currently active"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, all_active + ? "All hardware components are active" + : "Not all hardware components are active"); + } + } +} + +void ControllerManager::controller_manager_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + const std::string periodicity_stat_name = "periodicity"; + const auto cm_stats = periodicity_stats_.GetStatistics(); + stat.add("update_rate", std::to_string(get_update_rate())); + stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); + stat.add( + periodicity_stat_name + ".standard_deviation", std::to_string(cm_stats.standard_deviation)); + stat.add(periodicity_stat_name + ".min", std::to_string(cm_stats.min)); + stat.add(periodicity_stat_name + ".max", std::to_string(cm_stats.max)); + if (is_resource_manager_initialized()) + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); + } + else + { + if (robot_description_.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Waiting for robot description...."); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "Resource Manager is not initialized properly!"); + } + } + + const double periodicity_error = std::abs(cm_stats.average - get_update_rate()); + const std::string diag_summary = + "Controller Manager has bad periodicity : " + std::to_string(cm_stats.average) + + " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz"; + if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.error) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.warn || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); + } +} + +void ControllerManager::update_list_with_controller_chain( + const std::string & ctrl_name, std::vector::iterator controller_iterator, + bool append_to_controller) +{ + auto new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + if (new_ctrl_it == ordered_controllers_names_.end()) + { + RCLCPP_DEBUG(get_logger(), "Adding controller chain : %s", ctrl_name.c_str()); + + auto iterator = controller_iterator; + for (const auto & ctrl : controller_chain_spec_[ctrl_name].following_controllers) + { + auto it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl); + if (it != ordered_controllers_names_.end()) + { + if ( + std::distance(ordered_controllers_names_.begin(), it) < + std::distance(ordered_controllers_names_.begin(), iterator)) + { + iterator = it; + } + } + } + for (const auto & ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + { + auto it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl); + if (it != ordered_controllers_names_.end()) + { + if ( + std::distance(ordered_controllers_names_.begin(), it) > + std::distance(ordered_controllers_names_.begin(), iterator)) + { + iterator = it; + } + } + } + + if (append_to_controller) + { + ordered_controllers_names_.insert(iterator + 1, ctrl_name); + } + else + { + ordered_controllers_names_.insert(iterator, ctrl_name); + } + + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), + "\t[%s] Following controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].following_controllers.size()); + for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) + { + new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + RCLCPP_DEBUG(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); + update_list_with_controller_chain(flwg_ctrl, new_ctrl_it, true); + } + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), + "\t[%s] Preceding controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].preceding_controllers.size()); + for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + { + new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + RCLCPP_DEBUG(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); + update_list_with_controller_chain(preced_ctrl, new_ctrl_it, false); + } + } +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const ControllerSpec & controller) const @@ -2572,11 +4439,33 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( auto check_for_element = [](const auto & list, const auto & element) { return std::find(list.begin(), list.end(), element) != list.end(); }; +<<<<<<< HEAD rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); std::vector node_options_arguments = controller_node_options.arguments(); +======= + rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options(); + std::vector node_options_arguments = controller_node_options.arguments(); + + for (const std::string & arg : cm_node_options_.arguments()) + { + if (arg.find("__ns") != std::string::npos || arg.find("__node") != std::string::npos) + { + if ( + node_options_arguments.back() == RCL_REMAP_FLAG || + node_options_arguments.back() == RCL_SHORT_REMAP_FLAG) + { + node_options_arguments.pop_back(); + } + continue; + } + + node_options_arguments.push_back(arg); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & parameters_file : controller.info.parameters_files) { if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) @@ -2599,7 +4488,23 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back("use_sim_time:=true"); } +<<<<<<< HEAD + controller_node_options = controller_node_options.arguments(node_options_arguments); +======= + std::string arguments; + arguments.reserve(1000); + for (const auto & arg : node_options_arguments) + { + arguments.append(arg); + arguments.append(" "); + } + RCLCPP_INFO( + get_logger(), "Controller '%s' node arguments: %s", controller.info.name.c_str(), + arguments.c_str()); + controller_node_options = controller_node_options.arguments(node_options_arguments); + controller_node_options.use_global_arguments(false); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_node_options; } diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml new file mode 100644 index 0000000000..1bb9b152b1 --- /dev/null +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -0,0 +1,136 @@ +controller_manager: + update_rate: { + type: int, + default_value: 100, + read_only: true, + description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." + } + + hardware_components_initial_state: + unconfigured: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be only loaded when controller manager is started. These hardware components will need to be configured and activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + inactive: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be configured when controller manager is started. These hardware components will need to be activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + diagnostics: + threshold: + controller_manager: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + controllers: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index d6a0b0f3dd..311c21ff59 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -13,14 +13,21 @@ // limitations under the License. #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include #include #include "controller_manager/controller_manager.hpp" +<<<<<<< HEAD #include "rclcpp/rclcpp.hpp" +======= +#include "rclcpp/executors.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "realtime_tools/realtime_helpers.hpp" using namespace std::chrono_literals; @@ -42,6 +49,7 @@ int main(int argc, char ** argv) std::make_shared(); std::string manager_node_name = "controller_manager"; +<<<<<<< HEAD auto cm = std::make_shared(executor, manager_node_name); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); @@ -56,6 +64,26 @@ int main(int argc, char ** argv) cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); } } +======= + rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); + std::vector node_arguments = cm_node_options.arguments(); + for (int i = 1; i < argc; ++i) + { + if (node_arguments.empty() && std::string(argv[i]) != "--ros-args") + { + // A simple way to reject non ros args + continue; + } + node_arguments.push_back(argv[i]); + } + cm_node_options.arguments(node_arguments); + + 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); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const bool has_realtime = realtime_tools::has_realtime_kernel(); const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); std::string message; @@ -64,6 +92,32 @@ int main(int argc, char ** argv) RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); } +<<<<<<< HEAD +======= + rclcpp::Parameter cpu_affinity_param; + if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) + { + std::vector cpus = {}; + if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + cpus = {static_cast(cpu_affinity_param.as_int())}; + } + else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) + { + const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); + std::for_each( + cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), + [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); + } + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); + if (!affinity_result.first) + { + RCLCPP_WARN( + cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); + } + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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( @@ -73,6 +127,7 @@ int main(int argc, char ** argv) std::thread cm_thread( [cm, thread_priority, use_sim_time]() { +<<<<<<< HEAD if (realtime_tools::has_realtime_kernel()) { if (!realtime_tools::configure_sched_fifo(thread_priority)) @@ -98,6 +153,22 @@ int main(int argc, char ** argv) "No real-time kernel detected on this system. See " "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " "for details on how to enable realtime scheduling."); +======= + if (!realtime_tools::configure_sched_fifo(thread_priority)) + { + RCLCPP_WARN( + cm->get_logger(), + "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See " + "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " + "for details on how to enable realtime scheduling.", + errno, strerror(errno)); + } + else + { + RCLCPP_INFO( + cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", + thread_priority); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // for calculating sleep time @@ -107,7 +178,11 @@ int main(int argc, char ** argv) next_iteration_time{cm_now}; // for calculating the measured period of the loop +<<<<<<< HEAD rclcpp::Time previous_time = cm->now(); +======= + rclcpp::Time previous_time = cm->now() - period; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) while (rclcpp::ok()) { @@ -132,6 +207,11 @@ int main(int argc, char ** argv) std::this_thread::sleep_until(next_iteration_time); } } +<<<<<<< HEAD +======= + + cm->shutdown_async_controllers_and_components(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }); executor->add_node(cm); diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 9b38200f1c..7bebb25f25 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,26 +22,41 @@ #include #include #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" +<<<<<<< HEAD #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "rclcpp/rclcpp.hpp" +======= +#include "controller_manager_msgs/srv/switch_controller.hpp" + +#include "rclcpp/executors.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/utilities.hpp" #include "std_msgs/msg/string.hpp" #include "ros2_control_test_assets/descriptions.hpp" +<<<<<<< HEAD #include "test_controller_failed_init/test_controller_failed_init.hpp" namespace { const auto TIME = rclcpp::Time(0); +======= + +namespace +{ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto PERIOD = rclcpp::Duration::from_seconds(0.01); const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; @@ -67,6 +82,7 @@ class ControllerManagerFixture : public ::testing::Test public: explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, +<<<<<<< HEAD const bool & pass_urdf_as_parameter = false, const std::string & cm_namespace = "") : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) { @@ -101,6 +117,22 @@ class ControllerManagerFixture : public ::testing::Test pass_robot_description_to_cm_and_rm(robot_description_); } } +======= + const std::string & cm_namespace = "") + : robot_description_(robot_description) + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique( + rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), + executor_, TEST_CM_NAME, cm_namespace); + // We want to be able to not pass robot description immediately + if (!robot_description_.empty()) + { + pass_robot_description_to_cm_and_rm(robot_description_); + } + time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -119,7 +151,11 @@ class ControllerManagerFixture : public ::testing::Test { while (run_updater_) { +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); @@ -151,7 +187,10 @@ class ControllerManagerFixture : public ::testing::Test const std::future_status expected_future_status = std::future_status::timeout, const controller_interface::return_type expected_return = controller_interface::return_type::OK) { +<<<<<<< HEAD // First activation not possible because controller not configured +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); @@ -169,18 +208,28 @@ class ControllerManagerFixture : public ::testing::Test bool run_updater_; const std::string robot_description_; rclcpp::Time time_; +<<<<<<< HEAD const bool pass_urdf_as_parameter_; +======= + +protected: + rclcpp::Node::SharedPtr rm_node_ = std::make_shared("ResourceManager"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; class TestControllerManagerSrvs : public ControllerManagerFixture { public: +<<<<<<< HEAD TestControllerManagerSrvs() : ControllerManagerFixture( ros2_control_test_assets::minimal_robot_urdf, true) { } +======= + TestControllerManagerSrvs() {} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) void SetUp() override { @@ -194,9 +243,15 @@ class TestControllerManagerSrvs std::chrono::milliseconds(10), [&]() { +<<<<<<< HEAD cm_->read(TIME, PERIOD); cm_->update(TIME, PERIOD); cm_->write(TIME, PERIOD); +======= + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }); executor_->add_node(cm_); @@ -223,7 +278,11 @@ class TestControllerManagerSrvs while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) != rclcpp::FutureReturnCode::SUCCESS) { +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } else diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index ba1132e68b..8a83e840c6 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -33,8 +33,13 @@ controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { if ( +<<<<<<< HEAD get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return cmd_iface_cfg_; } @@ -49,9 +54,22 @@ controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { if ( +<<<<<<< HEAD get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { +======= + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto state_iface_cfg = state_iface_cfg_; + if (imu_sensor_) + { + auto imu_interfaces = imu_sensor_->get_state_interface_names(); + state_iface_cfg.names.insert( + state_iface_cfg.names.end(), imu_interfaces.begin(), imu_interfaces.end()); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return state_iface_cfg_; } else @@ -61,7 +79,12 @@ TestChainableController::state_interface_configuration() const } } +<<<<<<< HEAD controller_interface::return_type TestChainableController::update_reference_from_subscribers() +======= +controller_interface::return_type TestChainableController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { for (size_t i = 0; i < reference_interfaces_.size(); ++i) { @@ -95,6 +118,23 @@ controller_interface::return_type TestChainableController::update_and_write_comm { command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); } +<<<<<<< HEAD +======= + // If there is a command interface then integrate and set it to the exported state interface data + for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); + ++i) + { + state_interfaces_values_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + } + // If there is no command interface and if there is a state interface then just forward the same + // value as in the state interface + for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() && + command_interfaces_.empty(); + ++i) + { + state_interfaces_values_[i] = state_interfaces_[i].get_value(); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return controller_interface::return_type::OK; } @@ -149,6 +189,23 @@ CallbackReturn TestChainableController::on_cleanup( return CallbackReturn::SUCCESS; } +<<<<<<< HEAD +======= +std::vector +TestChainableController::on_export_state_interfaces() +{ + std::vector state_interfaces; + + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + { + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + } + + return state_interfaces; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::vector TestChainableController::on_export_reference_interfaces() { @@ -183,6 +240,34 @@ void TestChainableController::set_reference_interface_names( reference_interfaces_.resize(reference_interface_names.size(), 0.0); } +<<<<<<< HEAD +======= +void TestChainableController::set_exported_state_interface_names( + const std::vector & state_interface_names) +{ + exported_state_interface_names_ = state_interface_names; + + state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); +} + +void TestChainableController::set_imu_sensor_name(const std::string & name) +{ + if (!name.empty()) + { + imu_sensor_ = std::make_unique(name); + } +} + +std::vector TestChainableController::get_state_interface_data() const +{ + std::vector state_intr_data; + for (const auto & interface : state_interfaces_) + { + state_intr_data.push_back(interface.get_value()); + } + return state_intr_data; +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace test_chainable_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index a7205d3024..0d5221d52e 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -22,7 +22,12 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "controller_manager/visibility_control.h" #include "rclcpp/subscription.hpp" +<<<<<<< HEAD #include "realtime_tools/realtime_buffer.h" +======= +#include "realtime_tools/realtime_buffer.hpp" +#include "semantic_components/imu_sensor.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "std_msgs/msg/float64_multi_array.hpp" namespace test_chainable_controller @@ -35,6 +40,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface constexpr char TEST_CONTROLLER_NAME[] = "test_chainable_controller_name"; // corresponds to the name listed within the pluginlib xml constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_chainable_controller"; +<<<<<<< HEAD +======= +constexpr double CONTROLLER_DT = 0.001; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class TestChainableController : public controller_interface::ChainableControllerInterface { public: @@ -61,9 +70,19 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC +<<<<<<< HEAD std::vector on_export_reference_interfaces() override; controller_interface::return_type update_reference_from_subscribers() override; +======= + std::vector on_export_state_interfaces() override; + + CONTROLLER_MANAGER_PUBLIC + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -79,10 +98,27 @@ class TestChainableController : public controller_interface::ChainableController CONTROLLER_MANAGER_PUBLIC void set_reference_interface_names(const std::vector & reference_interface_names); +<<<<<<< HEAD +======= + CONTROLLER_MANAGER_PUBLIC + void set_exported_state_interface_names(const std::vector & state_interface_names); + + CONTROLLER_MANAGER_PUBLIC + void set_imu_sensor_name(const std::string & name); + + CONTROLLER_MANAGER_PUBLIC + std::vector get_state_interface_data() const; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) size_t internal_counter; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; +<<<<<<< HEAD +======= + std::vector exported_state_interface_names_; + std::unique_ptr imu_sensor_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index b6eac9b689..71b5c4370e 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -14,7 +14,11 @@ #include "test_controller.hpp" +<<<<<<< HEAD #include +======= +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "lifecycle_msgs/msg/state.hpp" @@ -25,13 +29,22 @@ TestController::TestController() : controller_interface::ControllerInterface(), cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE} { +<<<<<<< HEAD +======= + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { if ( +<<<<<<< HEAD get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return cmd_iface_cfg_; } @@ -45,8 +58,13 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { if ( +<<<<<<< HEAD get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return state_iface_cfg_; } @@ -58,6 +76,7 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con } controller_interface::return_type TestController::update( +<<<<<<< HEAD const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { ++internal_counter; @@ -68,6 +87,41 @@ controller_interface::return_type TestController::update( get_node()->get_logger(), "Setting value of command interface '%s' to %f", command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); command_interfaces_[i].set_value(external_commands_for_testing_[i]); +======= + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + if (is_async()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate()))); + } + update_period_ = period; + ++internal_counter; + + // set value to hardware to produce and test different behaviors there + if (!std::isnan(set_first_command_interface_value_to)) + { + command_interfaces_[0].set_value(set_first_command_interface_value_to); + // reset to be easier to test + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); + } + else + { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isfinite(external_commands_for_testing_[i])) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "External command value for command interface '%s' is not finite", + command_interfaces_[i].get_name().c_str()); + return controller_interface::return_type::ERROR; + } + RCLCPP_INFO( + get_node()->get_logger(), "Setting value of command interface '%s' to %f", + command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); + command_interfaces_[i].set_value(external_commands_for_testing_[i]); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return controller_interface::return_type::OK; @@ -107,6 +161,24 @@ void TestController::set_state_interface_configuration( state_iface_cfg_ = cfg; } +<<<<<<< HEAD +======= +std::vector TestController::get_state_interface_data() const +{ + std::vector state_intr_data; + for (const auto & interface : state_interfaces_) + { + state_intr_data.push_back(interface.get_value()); + } + return state_intr_data; +} + +void TestController::set_external_commands_for_testing(const std::vector & commands) +{ + external_commands_for_testing_ = commands; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace test_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index c88c11050d..70b0c0989c 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,7 +15,10 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include @@ -30,6 +33,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface // indicating the node name under which the controller node // is being loaded. constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name"; +<<<<<<< HEAD +======= +constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // corresponds to the name listed within the pluginlib xml constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller"; class TestController : public controller_interface::ControllerInterface @@ -65,6 +72,16 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); +<<<<<<< HEAD +======= + 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); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller @@ -74,6 +91,13 @@ class TestController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector external_commands_for_testing_; +<<<<<<< HEAD +======= + // enables external setting of values to command interfaces - used for simulation of hardware + // errors + double set_first_command_interface_value_to; + rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index 57d253d03c..90bf949e52 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -14,11 +14,14 @@ #include "test_controller_failed_init.hpp" +<<<<<<< HEAD #include #include #include "lifecycle_msgs/msg/transition.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace test_controller_failed_init { TestControllerFailedInit::TestControllerFailedInit() : controller_interface::ControllerInterface() @@ -31,6 +34,7 @@ TestControllerFailedInit::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } +<<<<<<< HEAD controller_interface::return_type TestControllerFailedInit::init( const std::string & /* controller_name */, const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/) @@ -38,6 +42,8 @@ controller_interface::return_type TestControllerFailedInit::init( return controller_interface::return_type::ERROR; } +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_interface::return_type TestControllerFailedInit::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index bfc2cd3cad..66cf51ced4 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -15,10 +15,14 @@ #ifndef TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_ #define TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_ +<<<<<<< HEAD #include #include #include "controller_manager/controller_manager.hpp" +======= +#include "controller_interface/controller_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "controller_manager/visibility_control.h" namespace test_controller_failed_init @@ -38,6 +42,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_MANAGER_PUBLIC virtual ~TestControllerFailedInit() = default; +<<<<<<< HEAD CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type init( const std::string & controller_name, const std::string & namespace_ = "", @@ -46,12 +51,24 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)) override; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_interface::InterfaceConfiguration command_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } +<<<<<<< HEAD +======= + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_interface::InterfaceConfiguration state_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a5566ee9b5..55f45d2535 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -15,6 +15,7 @@ #include #include #include +<<<<<<< HEAD #include #include @@ -22,6 +23,14 @@ #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +======= +#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" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "test_controller/test_controller.hpp" using ::testing::_; @@ -33,6 +42,37 @@ class TestControllerManagerWithStrictness { }; +<<<<<<< HEAD +======= +class TestControllerManagerRobotDescription +: public ControllerManagerFixture +{ +}; + +TEST_F(TestControllerManagerRobotDescription, controller_robot_description_update) +{ + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then load a new controller and see if the new controller + // gets the new description and the old controller still maintains the configuration + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + cm_->add_controller( + test_controller2, test_controller::TEST_CONTROLLER2_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller2->get_robot_description()); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) { const auto test_param = GetParam(); @@ -96,12 +136,21 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // configure controller { @@ -111,11 +160,21 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) } EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -126,7 +185,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -135,7 +198,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); // Start the real test controller, will take effect at the end of the update function @@ -150,17 +217,30 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } +<<<<<<< HEAD EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_GE(test_controller->internal_counter, 1u); size_t last_internal_counter = test_controller->internal_counter; @@ -176,7 +256,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) << "Controller is stopped at the end of update, so it should have done one more update"; { @@ -184,7 +268,13 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } +<<<<<<< HEAD EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto unload_future = std::async( std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); @@ -195,7 +285,213 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + EXPECT_EQ(1, test_controller.use_count()); +} + +TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) +{ + const auto test_param = GetParam(); + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // setup interface to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller->set_state_interface_configuration(state_itfs_cfg); + + controller_interface::InterfaceConfiguration cmd_itfs_cfg2; + cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg2.names.push_back(interface); + } + test_controller2->set_command_interface_configuration(cmd_itfs_cfg2); + + controller_interface::InterfaceConfiguration state_itfs_cfg2; + state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL; + test_controller2->set_state_interface_configuration(state_itfs_cfg2); + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/"); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controller + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(20)); + rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true)); + test_controller->get_node()->set_parameter(update_rate_parameter); + test_controller->get_node()->set_parameter(is_async_parameter); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(test_param.expected_return, switch_future.get()); + } + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); + + // Start the real test controller, will take effect at the end of the update function + start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + stop_controllers = {}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 0u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 1u); + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) + << "The first trigger cycle should have zero period"; + + const double exp_period = (cm_->get_clock()->now() - time_).seconds(); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 1u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 2u); + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period)))); + size_t last_internal_counter = test_controller->internal_counter; + + // Stop controller, will take effect at the end of the update function + start_controllers = {}; + stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "This shouldn't have updated as this is async and in the controller it is waiting before " + "updating the counter"; + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "Controller is stopped at the end of update, so it should have done one more update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + auto unload_future = std::async( + std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + + ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) + << "unload_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(1, test_controller.use_count()); } @@ -211,12 +507,21 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller @@ -226,10 +531,19 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) } EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -243,21 +557,34 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } +<<<<<<< HEAD EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); +======= + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle for (size_t i = 0; i < 25; i++) { EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); @@ -292,12 +619,21 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); @@ -308,10 +644,19 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd } EXPECT_EQ( controller_interface::return_type::OK, +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "Controller is not started"; + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -323,9 +668,16 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; +<<<<<<< HEAD EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + time_ += rclcpp::Duration::from_seconds(0.01); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is started at the end of update"; { @@ -333,6 +685,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } +<<<<<<< HEAD EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); const auto pre_internal_counter = test_controller->internal_counter; @@ -342,12 +695,54 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +======= + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + const auto pre_internal_counter = test_controller->internal_counter; + rclcpp::Rate loop_rate(cm_->get_update_rate()); + const auto cm_update_rate = cm_->get_update_rate(); + for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) + { + time_ += rclcpp::Duration::from_seconds(0.01); + 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 + // [cm_update_rate, 2*cm_update_rate) + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Ge(0.85 / cm_update_rate), testing::Lt((1.15 / cm_update_rate)))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf( + testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be // similarly incremented EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); +<<<<<<< HEAD EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); +======= + EXPECT_EQ(test_controller->get_update_rate(), cm_->get_update_rate()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto deactivate_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, @@ -357,8 +752,12 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "switch_controller should be blocking until next update cycle"; EXPECT_EQ( +<<<<<<< HEAD controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))) +======= + controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) << "Controller is stopped at the end of update, so it should have done one more update"; { ControllerManagerRunner cm_runner(this); @@ -395,7 +794,12 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller @@ -403,12 +807,26 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ControllerManagerRunner cm_runner(this); cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); } +<<<<<<< HEAD EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + 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))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Start controller, will take effect at the end of the update function const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; @@ -421,20 +839,34 @@ 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"; +<<<<<<< HEAD EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), 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))); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } +<<<<<<< HEAD EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); +======= + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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(); +<<<<<<< HEAD const auto controller_factor = (cm_update_rate / controller_update_rate); const auto expected_controller_update_rate = static_cast(std::round(cm_update_rate / static_cast(controller_factor))); @@ -452,10 +884,1148 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( test_controller->internal_counter - initial_counter, (expected_controller_update_rate * no_of_secs_passed)); +======= + 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_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); + 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))); + + if (test_controller->internal_counter - initial_counter > 0) + { + // 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_.seconds(), + testing::AllOf( + 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(); + } + else + { + // Check that the first cycle update period is zero + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) + { + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); + // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_THAT( + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 50.0); // 50 microseconds +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } } INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, +<<<<<<< HEAD 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 TestAsyncControllerUpdateRates +: public ControllerManagerFixture +{ +}; + +TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_and_stats) +{ + const unsigned int ctrl_update_rate = cm_->get_update_rate() / 2; + auto test_controller = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + test_controller->get_node()->set_parameter({"is_async", true}); + // configure controller + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } + ASSERT_TRUE(test_controller->is_async()); + 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))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + 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))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + 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_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); + 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))); + + // the async controllers will have to wait for one cycle to have the correct update period in + // the controller + if (test_controller->internal_counter - initial_counter > 1) + { + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + 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(); + } + // else + // { + // // Check that the first cycle update period is zero + // EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + // } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) + { + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); + // NOTE: here EXPECT_THAT is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_THAT( + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 12000); // more or less 12 milliseconds considering the waittime in the controller + } + } +} + +class TestControllerManagerFallbackControllers +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_same_controller_in_fallback_list) +{ + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + + const std::vector fallback_controllers = {test_controller_1_name, "random_ctrl2"}; + rclcpp::Parameter fallback_ctrls_parameter( + test_controller_1_name + std::string(".fallback_controllers"), fallback_controllers); + cm_->set_parameter(fallback_ctrls_parameter); + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + nullptr, + cm_->load_controller(test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME)); + } +} + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_controller_not_configured) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_command_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"random_non_existing_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_state_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.names = {"non_existing_state_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_valid_activation_if_one_or_more_fallback_controllers_are_already_active) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_3->set_command_interface_configuration(itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_3_name}; + controller_spec.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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.last_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + } + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + /// @note Here the order is important do not change the starting order + for (const auto & start_controller : {test_controller_3_name, test_controller_1_name}) + { + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {start_controller}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_multiple_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"joint1/position"}); + test_controller_4->set_exported_state_interface_names({"joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_3_name}; + controller_spec.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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.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.last_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_3 alone and it should fail + // as it doesn't have the chained state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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.last_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_2, test_controller_3 and + // test_controller_4, and now it should work as all the controllers are in the list + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + // It is expected to place all the chainable interfaces first, so they can make the interfaces + // available + controller_spec.info.fallback_controllers_names = { + test_controller_4_name, test_controller_3_name, test_controller_2_name}; + controller_spec.last_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing( + {std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_4->get_lifecycle_state().id()); + } +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_other_failing_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"modified_joint1/position"}); + test_controller_4->set_exported_state_interface_names({"modified_joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_4_name}; + controller_spec.last_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + 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.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.last_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + { + // Now the controller_1 is set with test_controller_2 and test_controller_4_name and it should + // fail as it has an non existing state interface Start controller, will take effect at the end + // of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + } + + // Now unload the controller_1 and set it up again with test_controller_3 and + // test_controller_4_name and it should fail as it has an non existing state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + cm_->unload_controller(test_controller_4_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_4_name}; + controller_spec.last_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp new file mode 100644 index 0000000000..936a2dd4c4 --- /dev/null +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -0,0 +1,741 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & node_namespace = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, node_namespace) + { + } +}; + +class TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetupAndConfigureControllers(int strictness) + { + test_controller_actuator = std::make_shared(); + cm_->add_controller( + test_controller_actuator, TEST_CONTROLLER_ACTUATOR_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_controller_actuator_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES}; + controller_interface::InterfaceConfiguration test_controller_actuator_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES}; + test_controller_actuator->set_command_interface_configuration( + test_controller_actuator_cmd_ifs_cfg); + test_controller_actuator->set_state_interface_configuration( + test_controller_actuator_state_ifs_cfg); + + test_controller_system = std::make_shared(); + cm_->add_controller( + test_controller_system, TEST_CONTROLLER_SYSTEM_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_system_controller_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES}; + controller_interface::InterfaceConfiguration test_system_controller_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SYSTEM_HARDWARE_STATE_INTERFACES}; + test_controller_system->set_command_interface_configuration(test_system_controller_cmd_ifs_cfg); + test_controller_system->set_state_interface_configuration(test_system_controller_state_ifs_cfg); + + test_broadcaster_all = std::make_shared(); + cm_->add_controller( + test_broadcaster_all, TEST_BROADCASTER_ALL_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_broadcaster_all_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::NONE, {}}; + controller_interface::InterfaceConfiguration test_broadcaster_all_state_ifs_cfg = { + controller_interface::interface_configuration_type::ALL, {}}; + test_broadcaster_all->set_command_interface_configuration(test_broadcaster_all_cmd_ifs_cfg); + test_broadcaster_all->set_state_interface_configuration(test_broadcaster_all_state_ifs_cfg); + + test_broadcaster_sensor = std::make_shared(); + cm_->add_controller( + test_broadcaster_sensor, TEST_BROADCASTER_SENSOR_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_broadcaster_sensor_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::NONE, {}}; + controller_interface::InterfaceConfiguration test_broadcaster_sensor_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SENSOR_HARDWARE_STATE_INTERFACES}; + test_broadcaster_sensor->set_command_interface_configuration( + test_broadcaster_sensor_cmd_ifs_cfg); + test_broadcaster_sensor->set_state_interface_configuration(test_broadcaster_sensor_ifs_cfg); + + // check if all controllers are added correctly + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_broadcaster_sensor->get_lifecycle_state().id()); + + // configure controllers + cm_->configure_controller(TEST_CONTROLLER_ACTUATOR_NAME); + cm_->configure_controller(TEST_CONTROLLER_SYSTEM_NAME); + cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); + cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_broadcaster_sensor->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME, + TEST_BROADCASTER_SENSOR_NAME}, + {}, strictness); + } + + static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; + static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; + static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; + static constexpr char TEST_BROADCASTER_SENSOR_NAME[] = "test_broadcaster_sensor"; + + std::shared_ptr test_controller_actuator; + std::shared_ptr test_controller_system; + std::shared_ptr test_broadcaster_all; + std::shared_ptr test_broadcaster_sensor; +}; + +TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to + // READ_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to READ_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in update method of the controllers but not in hardware + test_controller_actuator->external_commands_for_testing_[0] = + std::numeric_limits::quiet_NaN(); + test_controller_system->external_commands_for_testing_[0] = + std::numeric_limits::quiet_NaN(); + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::ERROR, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Executes the current cycle and returns ERROR"; + EXPECT_EQ( + test_controller_actuator->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Executes the current cycle and returns ERROR"; + EXPECT_EQ( + test_controller_system->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Cannot execute as it should be currently deactivated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter) + << "Cannot execute as it should be currently deactivated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Broadcaster all interfaces without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + + // The states shouldn't change as there are no more controller errors + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to + // WRITE_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to WRITE_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerManagerWithTestableCM, + testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 217f753327..43f140cba0 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -15,15 +15,25 @@ #include #include #include +<<<<<<< HEAD +======= +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include "controller_manager_test_common.hpp" #include "controller_interface/controller_interface.hpp" +<<<<<<< HEAD #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" #include "controller_manager_msgs/srv/list_controllers.hpp" +======= +#include "controller_manager_msgs/srv/list_controller_types.hpp" +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "controller_manager_msgs/srv/switch_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "test_chainable_controller/test_chainable_controller.hpp" @@ -34,6 +44,10 @@ using ::testing::Return; using ::testing::UnorderedElementsAre; using ListControllers = controller_manager_msgs::srv::ListControllers; +<<<<<<< HEAD +======= +using ListHardwareInterfaces = controller_manager_msgs::srv::ListHardwareInterfaces; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using TestController = test_controller::TestController; using TestChainableController = test_chainable_controller::TestChainableController; @@ -313,6 +327,7 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]); ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]); // activate controllers +<<<<<<< HEAD auto res = cm_->switch_controller( {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); @@ -327,6 +342,14 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) {test_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); ASSERT_EQ(res, controller_interface::return_type::OK); +======= + cm_->switch_controller( + {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // get controller list after activate result = call_service_and_wait(*client, request, srv_executor); // check test controller @@ -340,6 +363,10 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) test_chainable_controller::TEST_CONTROLLER_NAME, result->controller[0].chain_connections[0].name); ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size()); +<<<<<<< HEAD +======= + ASSERT_EQ("test_chainable_controller_name", result->controller[0].chain_connections[0].name); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]); ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]); } @@ -370,7 +397,12 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) std::weak_ptr test_controller_weak(test_controller); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -393,7 +425,13 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -417,13 +455,23 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Failed reload due to active controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor); ASSERT_FALSE(result->ok) << "Cannot reload if controllers are running"; +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; @@ -465,7 +513,11 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, +<<<<<<< HEAD cm_->get_loaded_controllers()[0].c->get_state().id()); +======= + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestControllerManagerSrvs, unload_controller_srv) @@ -491,10 +543,62 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); +} + +TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr unload_client = + srv_node->create_client( + "test_controller_manager/unload_controller"); + + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + // check the robot description + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then see that the controller maintains the old URDF until + // it is unloaded and loaded again + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // now unload and load the controller and see if the controller gets the new robot description + auto unload_request = std::make_shared(); + unload_request->name = test_controller::TEST_CONTROLLER_NAME; + auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); + + // now load it and check if it got the new robot description + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller->get_robot_description()); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(TestControllerManagerSrvs, configure_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; @@ -523,15 +627,27 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, +<<<<<<< HEAD cm_->get_loaded_controllers()[0].c->get_state().id()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); +======= + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // now unload the controller and check the state auto unload_request = std::make_shared(); unload_request->name = test_controller::TEST_CONTROLLER_NAME; ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -855,7 +971,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) /// && /// test_controller_name_2 -> chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 /// && +<<<<<<< HEAD /// test_controller_name_7 -> test_controller_name_8 +======= + /// test_controller_name_8 -> test_controller_name_7 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported /// from the controller B (or) the controller B is utilizing the expected interfaces exported from @@ -970,6 +1090,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // add controllers /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order +<<<<<<< HEAD cm_->add_controller( test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -998,6 +1119,39 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) cm_->add_controller( test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +======= + { + ControllerManagerRunner cm_runner(this); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // get controller list before configure auto result = call_service_and_wait(*client, request, srv_executor); @@ -1223,6 +1377,7 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) // add controllers /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order +<<<<<<< HEAD cm_->add_controller( test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1257,6 +1412,42 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) { ControllerManagerRunner cm_runner(this); +======= + { + ControllerManagerRunner cm_runner(this); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (auto random_ctrl : random_controllers_list) { cm_->add_controller( @@ -1494,7 +1685,13 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) } // Now shuffle the list to be able to configure controller later randomly +<<<<<<< HEAD std::random_shuffle(controllers_list.begin(), controllers_list.end()); +======= + std::random_device rnd; + std::mt19937 mers(rnd()); + std::shuffle(controllers_list.begin(), controllers_list.end(), mers); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { ControllerManagerRunner cm_runner(this); @@ -1559,6 +1756,167 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); } +<<<<<<< HEAD +======= +TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "test_controller_manager/list_hardware_interfaces"); + auto request = std::make_shared(); + + // create chained controller + auto test_chained_controller = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller->set_state_interface_configuration(chained_state_cfg); + test_chained_controller->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position", + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity", + "joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + + // add controllers + cm_->add_controller( + test_chained_controller, test_chainable_controller::TEST_CONTROLLER_NAME, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + // get hardware interface list before configure and loading + auto initial_result = call_service_and_wait(*client, request, srv_executor); + // As there is no controller, so all the interfaces should be available and unclaimed + for (const auto & cmd_itrf : initial_result->command_interfaces) + { + ASSERT_TRUE(cmd_itrf.is_available); + ASSERT_FALSE(cmd_itrf.is_claimed); + } + + // configure controllers + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + + // The interfaces should be same available and unclaimed until we activate the controllers + auto result = call_service_and_wait(*client, request, srv_executor); + + ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size()); + // There will be no increase in state interfaces + ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size()); + // As there is no controller, so all the interfaces should be available and unclaimed + for (const auto & cmd_itrf : result->command_interfaces) + { + // The controller command interface shouldn't be active until it's controller is active + if ( + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") + { + ASSERT_FALSE(cmd_itrf.is_available); + } + else + { + ASSERT_TRUE(cmd_itrf.is_available); + } + ASSERT_FALSE(cmd_itrf.is_claimed); + } + auto find_interface_in_list = [](const std::string & interface, auto & hw_interface_info) + { + return std::find_if( + hw_interface_info.begin(), hw_interface_info.end(), + [&](auto info) { return info.name == interface; }) != hw_interface_info.end(); + }; + ASSERT_TRUE(find_interface_in_list( + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position", + result->command_interfaces)); + ASSERT_TRUE(find_interface_in_list( + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity", + result->command_interfaces)); + + // activate controllers + cm_->switch_controller( + {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // On activating this chainable controller, we should see that there are 2 more command interfaces + // as this chainable controllers exports 2 reference interfaces + result = call_service_and_wait(*client, request, srv_executor); + + // There will be no increase upon activation + ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size()); + ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size()); + + for (const auto & cmd_itrf : result->command_interfaces) + { + ASSERT_TRUE(cmd_itrf.is_available); + // This interface is claimed by the chainable controller + if (cmd_itrf.name == "joint1/position") + { + EXPECT_TRUE(cmd_itrf.is_claimed); + } + else if ( + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") + { + // As these interfaces are exposed by the chainable controller and not yet claimed by other + // controllers + ASSERT_FALSE(cmd_itrf.is_claimed); + } + else + { + // Case for rest of the controllers + ASSERT_FALSE(cmd_itrf.is_claimed); + } + } + + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // Now as another controller using the interfaces of chainable controller is activated, those + // interfaces should reflect as claimed + result = call_service_and_wait(*client, request, srv_executor); + + for (const auto & cmd_itrf : result->command_interfaces) + { + ASSERT_TRUE(cmd_itrf.is_available); + // This interface is claimed by the chainable controller + if ( + cmd_itrf.name == "joint1/position" || cmd_itrf.name == "joint2/velocity" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") + { + ASSERT_TRUE(cmd_itrf.is_claimed); + } + else + { + // Case for rest of the controllers + ASSERT_FALSE(cmd_itrf.is_claimed); + } + } +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(TestControllerManagerSrvs, activate_chained_controllers_one_by_one) { /// The simulated controller chaining is: diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 102cbdfbd4..b7480b1d1e 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -20,8 +20,17 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" +<<<<<<< HEAD #include "ros2_control_test_assets/descriptions.hpp" +======= +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_controller/test_controller.hpp" + +const auto CONTROLLER_NAME = "test_controller"; +using test_controller::TEST_CONTROLLER_CLASS_NAME; +using strvec = std::vector; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class TestControllerManagerWithTestableCM; @@ -29,20 +38,36 @@ class TestableControllerManager : public controller_manager::ControllerManager { friend TestControllerManagerWithTestableCM; +<<<<<<< HEAD FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); +======= + FRIEND_TEST( + TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called); + FRIEND_TEST( + TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) public: TestableControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", +<<<<<<< HEAD const std::string & namespace_ = "") : controller_manager::ControllerManager( std::move(resource_manager), executor, manager_node_name, namespace_) +======= + const std::string & node_namespace = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, node_namespace) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } }; @@ -53,6 +78,7 @@ class TestControllerManagerWithTestableCM { public: // create cm with no urdf +<<<<<<< HEAD TestControllerManagerWithTestableCM() : ControllerManagerFixture("", false) { @@ -67,10 +93,70 @@ TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); +======= + TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} + + void prepare_controller() + { + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + test_controller_ = std::make_shared(); + cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(test_controller_, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_->get_lifecycle_state().id()); + } + + void configure_and_try_switch_that_returns_error() + { + // configure controller + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready, + controller_interface::return_type::ERROR); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); + } + + void try_successful_switch() + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, + controller_interface::return_type::OK); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_->get_lifecycle_state().id()); + } + + std::shared_ptr test_controller_; +}; + +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) +{ + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) +{ + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // mimic callback auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); +<<<<<<< HEAD ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } @@ -82,6 +168,117 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_ msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; cm_->robot_description_callback(msg); ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +======= + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); +} + +TEST_P( + TestControllerManagerWithTestableCM, + expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description) +{ + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + pass_robot_description_to_cm_and_rm( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + // wrong urdf + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); + // correct urdf + msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + + try_successful_switch(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_controller_manager_with_namespace.cpp b/controller_manager/test/test_controller_manager_with_namespace.cpp index 7d3d11c2e0..c2eb9e9cde 100644 --- a/controller_manager/test/test_controller_manager_with_namespace.cpp +++ b/controller_manager/test/test_controller_manager_with_namespace.cpp @@ -15,6 +15,7 @@ #include #include #include +<<<<<<< HEAD #include #include @@ -22,6 +23,12 @@ #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +======= +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "test_controller/test_controller.hpp" using ::testing::_; @@ -39,7 +46,12 @@ class TestControllerManagerWithNamespace executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( +<<<<<<< HEAD ros2_control_test_assets::minimal_robot_urdf, true, true), +======= + ros2_control_test_assets::minimal_robot_urdf, rm_node_->get_node_clock_interface(), + rm_node_->get_node_logging_interface(), true), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) executor_, TEST_CM_NAME, TEST_NAMESPACE); run_updater_ = false; } diff --git a/controller_manager/test/test_controller_spawner_wildcard_entries.yaml b/controller_manager/test/test_controller_spawner_wildcard_entries.yaml new file mode 100644 index 0000000000..1a05ac03d6 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_wildcard_entries.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + param1: 1.0 + param2: 2.0 + +wildcard_ctrl_3: + ros__parameters: + param3: 3.0 diff --git a/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml new file mode 100644 index 0000000000..bc93f162c1 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml @@ -0,0 +1,13 @@ +ctrl_1: + ros__parameters: + joint_names: ["joint1"] + +ctrl_2: + ros__parameters: + joint_names: ["joint2"] + fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"] + +ctrl_3: + ros__parameters: + joint_names: ["joint3"] + fallback_controllers: ["ctrl_9"] diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp index 86f3c8b76c..3557d2fc9d 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp @@ -14,11 +14,14 @@ #include "test_controller_with_interfaces.hpp" +<<<<<<< HEAD #include #include #include "lifecycle_msgs/msg/transition.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace test_controller_with_interfaces { TestControllerWithInterfaces::TestControllerWithInterfaces() diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index 96aaed11ce..031932e346 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -15,11 +15,16 @@ #ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ #define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +<<<<<<< HEAD #include #include #include "controller_interface/visibility_control.h" #include "controller_manager/controller_manager.hpp" +======= +#include "controller_interface/controller_interface.hpp" +#include "controller_manager/visibility_control.h" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace test_controller_with_interfaces { diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index d1ab196ff7..1b46523644 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -15,6 +15,7 @@ #include #include #include +<<<<<<< HEAD #include #include @@ -24,6 +25,15 @@ #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/parameter.hpp" +======= +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" @@ -43,6 +53,18 @@ class TestableTestChainableController : public test_chainable_controller::TestCh TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode); FRIEND_TEST( +<<<<<<< HEAD +======= + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_switching_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_deactivation_error_handling); + FRIEND_TEST( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); }; @@ -68,6 +90,18 @@ class TestableControllerManager : public controller_manager::ControllerManager TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode); FRIEND_TEST( +<<<<<<< HEAD +======= + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_switching_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_deactivation_error_handling); + FRIEND_TEST( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); public: @@ -75,9 +109,15 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", +<<<<<<< HEAD const std::string & namespace_ = "") : controller_manager::ControllerManager( std::move(resource_manager), executor, manager_node_name, namespace_) +======= + const std::string & node_namespace = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, node_namespace) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } }; @@ -87,18 +127,26 @@ class TestControllerChainingWithControllerManager public testing::WithParamInterface { public: +<<<<<<< HEAD TestControllerChainingWithControllerManager() : ControllerManagerFixture( ros2_control_test_assets::minimal_robot_urdf, true) { } +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) void SetUp() { executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( +<<<<<<< HEAD ros2_control_test_assets::diffbot_urdf, true, true), +======= + ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(), + rm_node_->get_node_logging_interface(), true), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) executor_, TEST_CM_NAME); run_updater_ = false; } @@ -110,7 +158,16 @@ class TestControllerChainingWithControllerManager pid_left_wheel_controller = std::make_shared(); pid_right_wheel_controller = std::make_shared(); diff_drive_controller = std::make_shared(); +<<<<<<< HEAD + position_tracking_controller = std::make_shared(); +======= + diff_drive_controller_two = std::make_shared(); position_tracking_controller = std::make_shared(); + position_tracking_controller_two = std::make_shared(); + odom_publisher_controller = std::make_shared(); + sensor_fusion_controller = std::make_shared(); + robot_localization_controller = std::make_shared(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { @@ -121,7 +178,11 @@ class TestControllerChainingWithControllerManager pid_left_wheel_controller->set_state_interface_configuration(pid_left_state_ifs_cfg); pid_left_wheel_controller->set_reference_interface_names({"velocity"}); +<<<<<<< HEAD // configure Left Wheel controller +======= + // configure Right Wheel controller +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = { @@ -140,6 +201,16 @@ class TestControllerChainingWithControllerManager diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); +<<<<<<< HEAD +======= + diff_drive_controller->set_exported_state_interface_names({"odom_x", "odom_y"}); + + // configure Diff Drive Two controller (Has same command interfaces as Diff Drive controller) + diff_drive_controller_two->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); + diff_drive_controller_two->set_state_interface_configuration(diff_drive_state_ifs_cfg); + diff_drive_controller_two->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); + diff_drive_controller_two->set_exported_state_interface_names({"odom_x", "odom_y"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // configure Position Tracking controller controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { @@ -149,10 +220,45 @@ class TestControllerChainingWithControllerManager // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel" position_tracking_controller->set_command_interface_configuration( position_tracking_cmd_ifs_cfg); +<<<<<<< HEAD +======= + + // configure Odometry Publisher controller + controller_interface::InterfaceConfiguration odom_and_fusion_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(DIFF_DRIVE_CONTROLLER) + "/odom_x", + std::string(DIFF_DRIVE_CONTROLLER) + "/odom_y"}}; + odom_publisher_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); + + // configure sensor fusion controller + sensor_fusion_controller->set_imu_sensor_name("base_imu"); + sensor_fusion_controller->set_state_interface_configuration(odom_and_fusion_ifs_cfg); + sensor_fusion_controller->set_exported_state_interface_names({"odom_x", "odom_y", "yaw"}); + + // configure Robot Localization controller + controller_interface::InterfaceConfiguration odom_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(SENSOR_FUSION_CONTROLLER) + "/odom_x", + std::string(SENSOR_FUSION_CONTROLLER) + "/odom_y", + std::string(SENSOR_FUSION_CONTROLLER) + "/yaw"}}; + robot_localization_controller->set_state_interface_configuration(odom_ifs_cfg); + robot_localization_controller->set_exported_state_interface_names({"actual_pose"}); + + // configure Position Tracking controller Two + controller_interface::InterfaceConfiguration position_tracking_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(ROBOT_LOCALIZATION_CONTROLLER) + "/actual_pose"}}; + // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel" + position_tracking_controller_two->set_command_interface_configuration( + position_tracking_cmd_ifs_cfg); + position_tracking_controller_two->set_state_interface_configuration( + position_tracking_state_ifs_cfg); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void CheckIfControllersAreAddedCorrectly() { +<<<<<<< HEAD EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, pid_left_wheel_controller.use_count()); EXPECT_EQ(2, pid_right_wheel_controller.use_count()); @@ -171,6 +277,46 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, position_tracking_controller->get_state().id()); +======= + EXPECT_EQ(9u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, pid_left_wheel_controller.use_count()); + EXPECT_EQ(2, pid_right_wheel_controller.use_count()); + EXPECT_EQ(2, diff_drive_controller.use_count()); + EXPECT_EQ(2, diff_drive_controller_two.use_count()); + EXPECT_EQ(2, position_tracking_controller.use_count()); + EXPECT_EQ(2, sensor_fusion_controller.use_count()); + EXPECT_EQ(2, robot_localization_controller.use_count()); + EXPECT_EQ(2, odom_publisher_controller.use_count()); + EXPECT_EQ(2, position_tracking_controller_two.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + pid_left_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + diff_drive_controller_two->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + position_tracking_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + robot_localization_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + position_tracking_controller_two->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // order or controller configuration is not important therefore we can reuse the same method @@ -178,13 +324,24 @@ class TestControllerChainingWithControllerManager { // Store initial values of command interfaces size_t number_of_cmd_itfs = cm_->resource_manager_->command_interface_keys().size(); +<<<<<<< HEAD +======= + size_t number_of_state_itfs = cm_->resource_manager_->state_interface_keys().size(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // configure chainable controller and check exported interfaces cm_->configure_controller(PID_LEFT_WHEEL); EXPECT_EQ( +<<<<<<< HEAD pid_left_wheel_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); +======= + pid_left_wheel_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & interface : {"pid_left_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -194,9 +351,16 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(PID_RIGHT_WHEEL); EXPECT_EQ( +<<<<<<< HEAD pid_right_wheel_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); +======= + pid_right_wheel_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & interface : {"pid_right_wheel_controller/velocity"}) { EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); @@ -205,9 +369,21 @@ class TestControllerChainingWithControllerManager } cm_->configure_controller(DIFF_DRIVE_CONTROLLER); +<<<<<<< HEAD EXPECT_EQ( diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); +======= + for (auto & x : cm_->resource_manager_->available_state_interfaces()) + { + RCLCPP_ERROR_STREAM(cm_->get_logger(), x); + } + EXPECT_EQ( + diff_drive_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 2); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & interface : {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}) @@ -216,18 +392,82 @@ class TestControllerChainingWithControllerManager EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } +<<<<<<< HEAD cm_->configure_controller(POSITION_TRACKING_CONTROLLER); EXPECT_EQ( position_tracking_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); +======= + for (const auto & interface : {"diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->state_interface_is_available(interface)); + } + + cm_->configure_controller(DIFF_DRIVE_CONTROLLER_TWO); + EXPECT_EQ( + diff_drive_controller_two->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + for (const auto & interface : + {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", + "diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + for (const auto & interface : {"diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->state_interface_is_available(interface)); + } + + cm_->configure_controller(POSITION_TRACKING_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); + + cm_->configure_controller(SENSOR_FUSION_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); + + cm_->configure_controller(ROBOT_LOCALIZATION_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); + + cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); + + cm_->configure_controller(POSITION_TRACKING_CONTROLLER_TWO); + EXPECT_EQ( + position_tracking_controller_two->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); + EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } template < typename T, typename std::enable_if< std::is_convertible::value, T>::type * = nullptr> +<<<<<<< HEAD void SetToChainedModeAndMakeReferenceInterfacesAvailable( std::shared_ptr & controller, const std::string & controller_name, const std::vector & reference_interfaces) @@ -241,6 +481,39 @@ class TestControllerChainingWithControllerManager EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); +======= + void SetToChainedModeAndMakeInterfacesAvailable( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & exported_state_interfaces, + const std::vector & reference_interfaces) + { + if (!exported_state_interfaces.empty() || !reference_interfaces.empty()) + { + controller->set_chained_mode(true); + EXPECT_TRUE(controller->is_in_chained_mode()); + if (!reference_interfaces.empty()) + { + // make reference interface command_interfaces available + cm_->resource_manager_->make_controller_reference_interfaces_available(controller_name); + for (const auto & interface : reference_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + } + if (!exported_state_interfaces.empty()) + { + // make state interface state_interfaces available + cm_->resource_manager_->make_controller_exported_state_interfaces_available( + controller_name); + for (const auto & interface : exported_state_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } @@ -251,7 +524,11 @@ class TestControllerChainingWithControllerManager void check_after_de_activate( std::shared_ptr & controller, const std::vector & claimed_command_itfs, size_t expected_internal_counter, const controller_interface::return_type expected_return, +<<<<<<< HEAD bool deactivated) +======= + bool deactivated, bool claimed_interfaces_from_hw = false) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { for (const auto & interface : claimed_command_itfs) { @@ -264,7 +541,20 @@ class TestControllerChainingWithControllerManager } else { +<<<<<<< HEAD EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); +======= + if (claimed_interfaces_from_hw) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)) + << "The interface : " << interface << " should be available but it is not available"; + } + else + { + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)) + << "The interface : " << interface << " shouldn't be available but it is available"; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } @@ -281,14 +571,34 @@ class TestControllerChainingWithControllerManager void ActivateAndCheckController( std::shared_ptr & controller, const std::string & controller_name, const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, +<<<<<<< HEAD const controller_interface::return_type expected_return = controller_interface::return_type::OK) { switch_test_controllers( {controller_name}, {}, test_param.strictness, std::future_status::timeout, expected_return); +======= + const controller_interface::return_type expected_return = controller_interface::return_type::OK, + const std::future_status expected_future_status = std::future_status::timeout) + { + switch_test_controllers( + {controller_name}, {}, test_param.strictness, expected_future_status, expected_return); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) check_after_de_activate( controller, claimed_command_itfs, expected_internal_counter, expected_return, false); } +<<<<<<< HEAD +======= + void ActivateController( + const std::string & controller_name, + const controller_interface::return_type expected_return = controller_interface::return_type::OK, + const std::future_status expected_future_status = std::future_status::timeout) + { + switch_test_controllers( + {controller_name}, {}, test_param.strictness, expected_future_status, expected_return); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) template < typename T, typename std::enable_if< std::is_convertible::value, @@ -296,12 +606,33 @@ class TestControllerChainingWithControllerManager void DeactivateAndCheckController( std::shared_ptr & controller, const std::string & controller_name, const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, +<<<<<<< HEAD const controller_interface::return_type expected_return = controller_interface::return_type::OK) { switch_test_controllers( {}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return); check_after_de_activate( controller, claimed_command_itfs, expected_internal_counter, expected_return, true); +======= + const bool claimed_interfaces_from_hw = false, + const controller_interface::return_type expected_return = controller_interface::return_type::OK, + const std::future_status expected_future_status = std::future_status::timeout) + { + switch_test_controllers( + {}, {controller_name}, test_param.strictness, expected_future_status, expected_return); + check_after_de_activate( + controller, claimed_command_itfs, expected_internal_counter, expected_return, true, + claimed_interfaces_from_hw); + } + + void DeactivateController( + const std::string & controller_name, + const controller_interface::return_type expected_return = controller_interface::return_type::OK, + const std::future_status expected_future_status = std::future_status::timeout) + { + switch_test_controllers( + {}, {controller_name}, test_param.strictness, expected_future_status, expected_return); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void UpdateAllControllerAndCheck( @@ -313,6 +644,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; +<<<<<<< HEAD cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -321,6 +653,19 @@ class TestControllerChainingWithControllerManager ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); +======= + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + // check if all controllers are updated + ASSERT_EQ(odom_publisher_controller->internal_counter, exp_internal_counter_pos_ctrl); + ASSERT_EQ(robot_localization_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); + ASSERT_EQ(sensor_fusion_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); + ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); + ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 8); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 12); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 10); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // check if values are set properly in controllers ASSERT_EQ( @@ -336,12 +681,31 @@ class TestControllerChainingWithControllerManager ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); +<<<<<<< HEAD +======= + EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); + EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(sensor_fusion_controller->state_interfaces_values_.size(), 3u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); +<<<<<<< HEAD +======= + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); @@ -350,17 +714,39 @@ class TestControllerChainingWithControllerManager pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); +<<<<<<< HEAD +======= + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // check data propagation through controllers and if individual controllers are working double chained_ctrl_calculation(double reference, double state) { return reference - state; } +<<<<<<< HEAD +======= + double chained_estimate_calculation(double reference, double state) + { + return (reference - state) * test_chainable_controller::CONTROLLER_DT; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) double hardware_calculation(double command) { return command / 2.0; } // set controllers' names static constexpr char PID_LEFT_WHEEL[] = "pid_left_wheel_controller"; static constexpr char PID_RIGHT_WHEEL[] = "pid_right_wheel_controller"; static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller"; +<<<<<<< HEAD static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller"; +======= + static constexpr char DIFF_DRIVE_CONTROLLER_TWO[] = "diff_drive_controller_two"; + static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller"; + static constexpr char SENSOR_FUSION_CONTROLLER[] = "sensor_fusion_controller"; + static constexpr char ROBOT_LOCALIZATION_CONTROLLER[] = "robot_localization_controller"; + static constexpr char ODOM_PUBLISHER_CONTROLLER[] = "odometry_publisher_controller"; + static constexpr char POSITION_TRACKING_CONTROLLER_TWO[] = "position_tracking_controller_two"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const std::vector PID_LEFT_WHEEL_REFERENCE_INTERFACES = { "pid_left_wheel_controller/velocity"}; @@ -368,6 +754,14 @@ class TestControllerChainingWithControllerManager "pid_right_wheel_controller/velocity"}; const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; +<<<<<<< HEAD +======= + const std::vector DIFF_DRIVE_STATE_INTERFACES = { + "diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}; + const std::vector SENSOR_FUSION_ESIMTATED_INTERFACES = { + "sensor_fusion_controller/odom_x", "sensor_fusion_controller/odom_y", + "sensor_fusion_controller/yaw"}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; @@ -380,7 +774,16 @@ class TestControllerChainingWithControllerManager std::shared_ptr pid_left_wheel_controller; std::shared_ptr pid_right_wheel_controller; std::shared_ptr diff_drive_controller; +<<<<<<< HEAD + std::shared_ptr position_tracking_controller; +======= + std::shared_ptr diff_drive_controller_two; + std::shared_ptr sensor_fusion_controller; + std::shared_ptr robot_localization_controller; + std::shared_ptr odom_publisher_controller; std::shared_ptr position_tracking_controller; + std::shared_ptr position_tracking_controller_two; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) testing::WithParamInterface::ParamType test_param; @@ -391,6 +794,19 @@ class TestControllerChainingWithControllerManager double EXP_RIGHT_WHEEL_HW_STATE = 0.0; double EXP_LEFT_WHEEL_REF = 0.0; double EXP_RIGHT_WHEEL_REF = 0.0; +<<<<<<< HEAD +======= + double EXP_STATE_ODOM_X = 0.0; + double EXP_STATE_ODOM_Y = 0.0; + + // Expected behaviors struct used in chaining activation/deactivation tests + struct ExpectedBehaviorStruct + { + controller_interface::return_type return_type; + std::future_status future_status; + uint8_t state; + }; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; // The tests are implementing example of chained-control for DiffDrive robot shown here: @@ -409,22 +825,55 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) diff_drive_controller, DIFF_DRIVE_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( +<<<<<<< HEAD +======= + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) pid_left_wheel_controller, PID_LEFT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD +======= + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CheckIfControllersAreAddedCorrectly(); ConfigureAndCheckControllers(); +<<<<<<< HEAD SetToChainedModeAndMakeReferenceInterfacesAvailable( pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeReferenceInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeReferenceInterfacesAvailable( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); +======= + SetToChainedModeAndMakeInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, {}, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, + DIFF_DRIVE_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, SENSOR_FUSION_ESIMTATED_INTERFACES, {}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( @@ -434,17 +883,41 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); +<<<<<<< HEAD +======= + // Before starting check that the internal counters are set to zero + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 0u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 0u); + ASSERT_EQ(diff_drive_controller->internal_counter, 0u); + ASSERT_EQ(diff_drive_controller_two->internal_counter, 0u); + ASSERT_EQ(position_tracking_controller->internal_counter, 0u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 0u); + ASSERT_EQ(odom_publisher_controller->internal_counter, 0u); + ASSERT_EQ(robot_localization_controller->internal_counter, 0u); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER // (otherwise, interface will be missing) ActivateAndCheckController( pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); +<<<<<<< HEAD + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); +======= + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 1u); ActivateAndCheckController( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 1u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); // Diff-Drive Controller claims the reference interfaces of PID controllers ActivateAndCheckController( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); +<<<<<<< HEAD +======= + ASSERT_EQ(diff_drive_controller->internal_counter, 1u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); @@ -452,6 +925,33 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ActivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); +<<<<<<< HEAD +======= + ASSERT_EQ(position_tracking_controller->internal_counter, 1u); + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // Sensor Controller uses exported state interfaces of Diff-Drive Controller and IMU + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 1u); + ASSERT_EQ(position_tracking_controller->internal_counter, 3u); + ASSERT_EQ(diff_drive_controller->internal_counter, 5u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 7u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 9u); + + // Robot localization Controller uses exported state interfaces of Diff-Drive Controller + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ASSERT_EQ(robot_localization_controller->internal_counter, 1u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 3u); + ASSERT_EQ(position_tracking_controller->internal_counter, 5u); + ASSERT_EQ(diff_drive_controller->internal_counter, 7u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 9u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 11u); + + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) { @@ -459,9 +959,19 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } +<<<<<<< HEAD ASSERT_EQ(diff_drive_controller->internal_counter, 3u); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); +======= + ASSERT_EQ(odom_publisher_controller->internal_counter, 1u); + ASSERT_EQ(robot_localization_controller->internal_counter, 3u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 5u); + ASSERT_EQ(position_tracking_controller->internal_counter, 7u); + ASSERT_EQ(diff_drive_controller->internal_counter, 9u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 11u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 13u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // update controllers std::vector reference = {32.0, 128.0}; @@ -474,25 +984,59 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +<<<<<<< HEAD ASSERT_EQ(position_tracking_controller->internal_counter, 2u); +======= + ASSERT_EQ(position_tracking_controller->internal_counter, 8u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +<<<<<<< HEAD ASSERT_EQ(diff_drive_controller->internal_counter, 4u); +======= + ASSERT_EQ(diff_drive_controller->internal_counter, 10u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); +<<<<<<< HEAD // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); +======= + // run the update cycles of the robot localization and odom publisher controller + sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); + robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(robot_localization_controller->internal_counter, 4u); + odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); + EXP_STATE_ODOM_X = + chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 / dt + EXP_STATE_ODOM_Y = + chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 / dt + ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data().size(), 2u); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + + // update PID controllers that are writing to hardware + pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 14u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 12u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // update hardware ('read' is sufficient for test hardware) cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -504,6 +1048,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); +<<<<<<< HEAD +======= + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // 128 - 0 EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); @@ -511,8 +1061,19 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); +<<<<<<< HEAD + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); +======= + ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); + ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); + ASSERT_EQ(robot_localization_controller->internal_counter, 4u); // DiffDrive uses the same state ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // The state doesn't change wrt to any data from the hardware calculation + ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); + ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // update all controllers at once and see that all have expected values --> also checks the order // of controller execution @@ -538,11 +1099,32 @@ TEST_P( diff_drive_controller, DIFF_DRIVE_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( +<<<<<<< HEAD +======= + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) pid_left_wheel_controller, PID_LEFT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD +======= + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CheckIfControllersAreAddedCorrectly(); @@ -565,6 +1147,10 @@ TEST_P( EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); +<<<<<<< HEAD +======= + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // DiffDrive (preceding) controller is activated --> PID controller in chained mode ActivateAndCheckController( @@ -572,6 +1158,10 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); +<<<<<<< HEAD +======= + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // PositionController is activated --> all following controller in chained mode ActivateAndCheckController( @@ -580,12 +1170,47 @@ TEST_P( EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); +<<<<<<< HEAD +======= + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + for (const auto & interface : {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + for (const auto & interface : {"diff_drive_controller/odom_x", "diff_drive_controller/odom_y"}) + { + EXPECT_TRUE(cm_->resource_manager_->state_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->state_interface_is_available(interface)); + } + + // Sensor fusion Controller uses exported state interfaces of Diff-Drive Controller and IMU + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Robot localization Controller uses exported state interfaces of sensor fusion Controller + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + + // Odometry Publisher Controller uses exported state interfaces of Diff-Drive Controller + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + ASSERT_EQ(robot_localization_controller->internal_counter, 3u); + ASSERT_EQ(odom_publisher_controller->internal_counter, 1u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) UpdateAllControllerAndCheck({32.0, 128.0}, 2u); UpdateAllControllerAndCheck({1024.0, 4096.0}, 3u); // Test switch 'from chained mode' when controllers are deactivated +<<<<<<< HEAD // PositionController is deactivated --> DiffDrive controller is not in chained mode anymore DeactivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, @@ -609,6 +1234,912 @@ TEST_P( EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); +======= + // PositionController is deactivated --> DiffDrive controller still continues in chained mode + // As the DiffDriveController is in chained mode, right now we tend to also deactivate + // the other controllers that rely on the DiffDriveController exported interfaces + DeactivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 10u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + // SensorFusionController continues to stay in the chained mode as it is still using the state + // interfaces + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the robot localization controller and see that the sensor fusion controller is still + // active but not in the chained mode + DeactivateAndCheckController( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 8u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the odometry publisher controller + DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 8u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active + // but not in the chained mode + DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 14u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the diff_drive_controller as all it's following controllers that uses it's + // interfaces are deactivated + DeactivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 20u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + + // all controllers are deactivated --> chained mode is not changed + DeactivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 26u, true); + DeactivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 26u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); +} + +TEST_P( + TestControllerChainingWithControllerManager, test_chained_controllers_activation_error_handling) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Test Case 1: Trying to activate a preceding controller when following controller + // is not activated --> return error (If STRICT); Preceding controller is still inactive. + + static std::unordered_map expected = { + {controller_manager_msgs::srv::SwitchController::Request::STRICT, + {controller_interface::return_type::ERROR, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}, + {controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + {controller_interface::return_type::OK, std::future_status::timeout, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; + + // Attempt to activate preceding controller (diff-drive controller) with no check + ActivateController( + DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + + // Check if the controller activated (Should not be activated) + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + + ActivateController( + SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + // Check if the controller activated (Should not be activated) + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + + // Test Case 2: Try to activate a preceding controller the same time when trying to + // deactivate a following controller (using switch_controller function) + // --> return error; preceding controller is not activated, + // BUT following controller IS deactivated + + // Activate and check the following controllers: + // (pid_left_wheel_controller) (pid_right_wheel_controller) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + + // Attempt to activate a preceding controller (diff-drive controller + remaining) + // while trying to deactivate a following controller + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER, ODOM_PUBLISHER_CONTROLLER, SENSOR_FUSION_CONTROLLER}, {PID_RIGHT_WHEEL}, + test_param.strictness, expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + + // Preceding controller should stay deactivated and following controller + // should be deactivated (if BEST_EFFORT) + // If STRICT, preceding controller should stay deactivated and following controller + // should stay activated + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); +} + +TEST_P( + TestControllerChainingWithControllerManager, + test_chained_controllers_activation_switching_error_handling) +{ + // Test Case 3: In terms of current implementation. + // Example: Need two diff drive controllers, one should be deactivated, + // and the other should be activated. Following controller should stay in activated state. + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // Activate the following controller and the preceding controllers + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + + // Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other + // depending controllers are active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller_two->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller_two->get_lifecycle_state().id()); + + // Deactivate position_tracking_controller and activate position_tracking_controller_two + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER_TWO}, {POSITION_TRACKING_CONTROLLER}, test_param.strictness, + std::future_status::timeout, controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller_two->get_lifecycle_state().id()); + + // Now deactivate the position_tracking_controller_two and it should be in inactive state + switch_test_controllers( + {}, {POSITION_TRACKING_CONTROLLER_TWO}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller_two->get_lifecycle_state().id()); + + // Activate it again and deactivate it others to see if we can deactivate it in a group + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER_TWO}, {}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller_two->get_lifecycle_state().id()); + + // Deactivate the first preceding controller (diff_drive_controller) and + // activate the other preceding controller (diff_drive_controller_two) + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER_TWO}, + {POSITION_TRACKING_CONTROLLER_TWO, DIFF_DRIVE_CONTROLLER, SENSOR_FUSION_CONTROLLER, + ROBOT_LOCALIZATION_CONTROLLER, ODOM_PUBLISHER_CONTROLLER}, + test_param.strictness, std::future_status::timeout, controller_interface::return_type::OK); + + // Following controllers should stay active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); + // The original preceding controller (diff_drive_controller) should be inactive while + // the other preceding controller should be active (diff_drive_controller_two) + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller_two->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller_two->get_lifecycle_state().id()); + + // Activate all the controllers again in group and deactivate the diff_drive_controller_two + switch_test_controllers( + {POSITION_TRACKING_CONTROLLER_TWO, DIFF_DRIVE_CONTROLLER, SENSOR_FUSION_CONTROLLER, + ROBOT_LOCALIZATION_CONTROLLER, ODOM_PUBLISHER_CONTROLLER}, + {DIFF_DRIVE_CONTROLLER_TWO}, test_param.strictness, std::future_status::timeout, + controller_interface::return_type::OK); + // Following controllers should stay active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller_two->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + // This is false, because it only uses the state interfaces and exposes state interfaces + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + position_tracking_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller_two->get_lifecycle_state().id()); +} + +TEST_P( + TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Activate following controllers + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + + // Test Case 5: Deactivating a preceding controller that is not active --> return error; + // all controller stay in the same state + + // There is different error and timeout behavior depending on strictness + static std::unordered_map expected = { + {controller_manager_msgs::srv::SwitchController::Request::STRICT, + {controller_interface::return_type::ERROR, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}, + {controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + {controller_interface::return_type::OK, std::future_status::timeout, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; + + // Verify preceding controller (diff_drive_controller) is inactive + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Attempt to deactivate inactive controller (diff_drive_controller) + DeactivateController( + DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + DeactivateController( + SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + DeactivateController( + ODOM_PUBLISHER_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + DeactivateController( + ROBOT_LOCALIZATION_CONTROLLER, expected.at(test_param.strictness).return_type, + std::future_status::ready); + + // Check to see preceding controller (diff_drive_controller) is still inactive and + // following controllers (pid_left_wheel_controller) (pid_left_wheel_controller) are still active + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Test Case 6: following controller is deactivated but preceding controller will be activated + // --> return error; controllers stay in the same state + + switch_test_controllers( + {DIFF_DRIVE_CONTROLLER}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, + expected.at(test_param.strictness).future_status, + expected.at(test_param.strictness).return_type); + + // Preceding controller should stay deactivated and following controller + // should be deactivated (if BEST_EFFORT) + // If STRICT, preceding controller should stay deactivated and following controller + // should stay activated + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_left_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + + // Test Case 7: following controller deactivation but preceding controller is active + // --> return error; controllers stay in the same state as they were + + // Activate all controllers for this test + ActivateController( + PID_LEFT_WHEEL, expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + ActivateController( + PID_RIGHT_WHEEL, expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + ActivateController( + DIFF_DRIVE_CONTROLLER, controller_interface::return_type::OK, std::future_status::timeout); + + // Expect all controllers to be active + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + + // Attempt to deactivate following controllers + switch_test_controllers( + {}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + + // All controllers should still be active + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + + // Attempt to deactivate a following controller + switch_test_controllers( + {}, {PID_RIGHT_WHEEL}, test_param.strictness, std::future_status::ready, + expected.at(test_param.strictness).return_type); + + // All controllers should still be active + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); +} + +TEST_P( + TestControllerChainingWithControllerManager, + test_chained_controllers_deactivation_switching_error_handling) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Activate the following controller and the preceding controllers + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + + // check once active that they are in chained mode + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + + // Verify that initially all of them are in active state + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + position_tracking_controller->get_lifecycle_state().id()); + + // There is different error and timeout behavior depending on strictness + static std::unordered_map expected = { + {controller_manager_msgs::srv::SwitchController::Request::STRICT, + {controller_interface::return_type::ERROR, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}}, + {controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + {controller_interface::return_type::OK, std::future_status::ready, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}}; + + // Test switch 'from chained mode' when controllers are deactivated and possible combination of + // disabling controllers that use reference/state interfaces of the other controller. This is + // important to check that deactivation is not trigger irrespective of the type + // (reference/state) interface that is shared among the other controllers + + // PositionController is deactivated --> DiffDrive controller still continues in chained mode + // As the DiffDriveController is in chained mode, right now we tend to also deactivate + // the other controllers that rely on the DiffDriveController expected interfaces + DeactivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 2u, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + // SensorFusionController continues to stay in the chained mode as it is still using the state + // interfaces + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // DiffDrive (preceding) controller is activated --> PID controller in chained mod + // Let's try to deactivate the diff_drive_control, it should fail as there are still other + // controllers that use it's resources + DeactivateController( + DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + check_after_de_activate( + diff_drive_controller, DIFF_DRIVE_CLAIMED_INTERFACES, 0u, + controller_interface::return_type::ERROR, true, true); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the + // robot localization controller is still active + DeactivateAndCheckController( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u, true, + expected.at(test_param.strictness).return_type, + expected.at(test_param.strictness).future_status); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the robot localization controller and see that the sensor fusion controller is still + // active but not in the chained mode + DeactivateAndCheckController( + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the sensor_fusion controller and this should be successful as there are no other + // controllers using it's interfaces + DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the odometry publisher controller and now the diff_drive should continue active but + // not in chained mode + DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 0u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); + + // Deactivate the diff_drive_controller as all it's following controllers that uses it's + // interfaces are deactivated + DeactivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 0u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + + // all controllers are deactivated --> chained mode is not changed + DeactivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 0u, true); + DeactivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 0u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); + ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + robot_localization_controller->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) @@ -620,6 +2151,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add pid_left_wheel_controller, PID_LEFT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( +<<<<<<< HEAD +======= + robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) pid_right_wheel_controller, PID_RIGHT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( @@ -628,17 +2165,42 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add cm_->add_controller( diff_drive_controller, DIFF_DRIVE_CONTROLLER, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD +======= + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller_two, POSITION_TRACKING_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CheckIfControllersAreAddedCorrectly(); ConfigureAndCheckControllers(); +<<<<<<< HEAD SetToChainedModeAndMakeReferenceInterfacesAvailable( pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeReferenceInterfacesAvailable( pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); SetToChainedModeAndMakeReferenceInterfacesAvailable( diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); +======= + SetToChainedModeAndMakeInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, {}, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, {}, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_STATE_INTERFACES, + DIFF_DRIVE_REFERENCE_INTERFACES); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_THROW( cm_->resource_manager_->make_controller_reference_interfaces_available( @@ -666,6 +2228,12 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add ActivateAndCheckController( position_tracking_controller, POSITION_TRACKING_CONTROLLER, POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); +<<<<<<< HEAD +======= + ActivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 1u); + ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u); + ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // 'rot_z' reference interface is not claimed for (const auto & interface : {"diff_drive_controller/rot_z"}) { @@ -673,13 +2241,26 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } +<<<<<<< HEAD ASSERT_EQ(diff_drive_controller->internal_counter, 3u); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); +======= + ASSERT_EQ(diff_drive_controller->internal_counter, 9u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 11u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 13u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // update controllers std::vector reference = {32.0, 128.0}; +<<<<<<< HEAD +======= + sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + robot_localization_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // update 'Position Tracking' controller for (auto & value : diff_drive_controller->reference_interfaces_) { @@ -688,14 +2269,22 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +<<<<<<< HEAD ASSERT_EQ(position_tracking_controller->internal_counter, 2u); +======= + ASSERT_EQ(position_tracking_controller->internal_counter, 8u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +<<<<<<< HEAD ASSERT_EQ(diff_drive_controller->internal_counter, 4u); +======= + ASSERT_EQ(diff_drive_controller->internal_counter, 10u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 @@ -704,9 +2293,15 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +<<<<<<< HEAD ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); +======= + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 14u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 12u); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // update hardware ('read' is sufficient for test hardware) cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 37e38339ba..82e85e9491 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -22,7 +22,10 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" +<<<<<<< HEAD #include "controller_manager_msgs/srv/list_controllers.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -37,6 +40,7 @@ using hardware_interface::lifecycle_state_names::INACTIVE; using hardware_interface::lifecycle_state_names::UNCONFIGURED; using hardware_interface::lifecycle_state_names::UNKNOWN; +<<<<<<< HEAD using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -50,6 +54,21 @@ using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +======= +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_PLUGIN_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_PLUGIN_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; @@ -63,12 +82,19 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs void SetUp() override { executor_ = std::make_shared(); +<<<<<<< HEAD cm_ = std::make_shared( std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; cm_->set_parameter( rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); +======= + cm_ = std::make_shared(executor_, TEST_CM_NAME); + run_updater_ = false; + + SetUpSrvsCMExecutor(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -76,6 +102,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); +<<<<<<< HEAD std::string robot_description = ""; cm_->get_parameter("robot_description", robot_description); if (robot_description.empty()) @@ -89,15 +116,26 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); +======= + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void check_component_fileds( const controller_manager_msgs::msg::HardwareComponentState & component, +<<<<<<< HEAD const std::string & name, const std::string & type, const std::string & class_type, +======= + const std::string & name, const std::string & type, const std::string & plugin_name, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const uint8_t state_id, const std::string & state_label) { EXPECT_EQ(component.name, name) << "Component has unexpected name."; EXPECT_EQ(component.type, type) +<<<<<<< HEAD << "Component " << name << " from plugin " << class_type << " has wrong type."; EXPECT_EQ(component.class_type, class_type) << "Component " << name << " (" << type << ") has unexpected class_type."; @@ -106,6 +144,16 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs << " has wrong state_id."; EXPECT_EQ(component.state.label, state_label) << "Component " << name << " (" << type << ") from plugin " << class_type +======= + << "Component " << name << " from plugin " << plugin_name << " has wrong type."; + EXPECT_EQ(component.plugin_name, plugin_name) + << "Component " << name << " (" << type << ") has unexpected plugin_name."; + EXPECT_EQ(component.state.id, state_id) + << "Component " << name << " (" << type << ") from plugin " << plugin_name + << " has wrong state_id."; + EXPECT_EQ(component.state.label, state_label) + << "Component " << name << " (" << type << ") from plugin " << plugin_name +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) << " has wrong state_label."; } @@ -147,7 +195,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { check_component_fileds( component, TEST_ACTUATOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_TYPE, +<<<<<<< HEAD TEST_ACTUATOR_HARDWARE_CLASS_TYPE, hw_state_ids[0], hw_state_labels[0]); +======= + TEST_ACTUATOR_HARDWARE_PLUGIN_NAME, hw_state_ids[0], hw_state_labels[0]); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) check_interfaces( component.command_interfaces, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, hw_itfs_available_status[0][0], hw_itfs_claimed_status[0][0]); @@ -159,7 +211,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { check_component_fileds( component, TEST_SENSOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_TYPE, +<<<<<<< HEAD TEST_SENSOR_HARDWARE_CLASS_TYPE, hw_state_ids[1], hw_state_labels[1]); +======= + TEST_SENSOR_HARDWARE_PLUGIN_NAME, hw_state_ids[1], hw_state_labels[1]); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) check_interfaces( component.command_interfaces, TEST_SENSOR_HARDWARE_COMMAND_INTERFACES, hw_itfs_available_status[1][0], hw_itfs_claimed_status[1][0]); @@ -171,7 +227,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { check_component_fileds( component, TEST_SYSTEM_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_TYPE, +<<<<<<< HEAD TEST_SYSTEM_HARDWARE_CLASS_TYPE, hw_state_ids[2], hw_state_labels[2]); +======= + TEST_SYSTEM_HARDWARE_PLUGIN_NAME, hw_state_ids[2], hw_state_labels[2]); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) check_interfaces( component.command_interfaces, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, hw_itfs_available_status[2][0], hw_itfs_claimed_status[2][0]); @@ -207,7 +267,12 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) { // Default status after start: +<<<<<<< HEAD // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read +======= + // checks if "hardware_components_initial_state.unconfigured" and + // "hardware_components_initial_state.inactive" are correctly read +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) list_hardware_components_and_check( // actuator, sensor, system @@ -368,6 +433,7 @@ class TestControllerManagerHWManagementSrvsWithoutParams void SetUp() override { executor_ = std::make_shared(); +<<<<<<< HEAD cm_ = std::make_shared( std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; @@ -387,6 +453,13 @@ class TestControllerManagerHWManagementSrvsWithoutParams auto msg = std_msgs::msg::String(); msg.data = robot_description; +======= + cm_ = std::make_shared(executor_, TEST_CM_NAME); + run_updater_ = false; + + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); @@ -395,8 +468,14 @@ class TestControllerManagerHWManagementSrvsWithoutParams TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware) { +<<<<<<< HEAD // "configure_components_on_start" and "activate_components_on_start" are not set (empty) // therefore all hardware components are active +======= + // "hardware_components_initial_state.unconfigured" and + // "hardware_components_initial_state.inactive" are not set (empty). Therefore, all hardware + // components are active. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) list_hardware_components_and_check( // actuator, sensor, system std::vector( @@ -416,6 +495,7 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } +<<<<<<< HEAD // BEGIN: Deprecated parameters class TestControllerManagerHWManagementSrvsOldParameters @@ -477,3 +557,5 @@ TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_compone })); } // END: Deprecated parameters +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp index b9f3027602..02ebb88518 100644 --- a/controller_manager/test/test_hardware_spawner.cpp +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -232,7 +232,11 @@ class TestHardwareSpawnerWithNamespacedCM public: TestHardwareSpawnerWithNamespacedCM() : ControllerManagerFixture( +<<<<<<< HEAD ros2_control_test_assets::minimal_robot_urdf, false, "foo_namespace"), +======= + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace"), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) RMServiceCaller("foo_namespace/" + std::string(TEST_CM_NAME)) { cm_->set_parameter( diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ed443ea3d4..fcb751fbea 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -19,11 +19,18 @@ #include #include +<<<<<<< HEAD #include "controller_interface/controller_interface.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "test_controller/test_controller.hpp" +<<<<<<< HEAD +======= +#include "test_controller_failed_init/test_controller_failed_init.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using test_controller::TEST_CONTROLLER_CLASS_NAME; using ::testing::_; @@ -63,6 +70,7 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) ASSERT_NE(controller_if, nullptr); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); controller_if->get_node()->set_parameter({"update_rate", 1337}); @@ -71,6 +79,18 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1337u, controller_if->get_update_rate()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); + + controller_if->get_node()->set_parameter({"update_rate", 50}); + + cm_->configure_controller("test_controller_01"); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); + + EXPECT_EQ(50u, controller_if->get_update_rate()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } class TestLoadedController : public TestLoadController @@ -119,10 +139,19 @@ TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); cm_->configure_controller(CONTROLLER_NAME_1); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); + + cm_->configure_controller(CONTROLLER_NAME_1); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) @@ -131,7 +160,12 @@ TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) @@ -144,7 +178,12 @@ TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) // Stop controller stop_test_controller(test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) @@ -152,14 +191,24 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) const auto test_param = GetParam(); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // Test starting unconfigured controller, and starting configured afterwards start_test_controller( test_param.strictness, std::future_status::ready, test_param.expected_return); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Activate configured controller { @@ -167,12 +216,23 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) cm_->configure_controller(CONTROLLER_NAME_1); } start_test_controller(test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Stop controller stop_test_controller(test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } @@ -185,7 +245,12 @@ TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) // Can not configure active controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) @@ -193,7 +258,12 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) const auto test_param = GetParam(); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Shutdown controller on purpose for testing ASSERT_EQ( @@ -206,7 +276,12 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) // Can not configure finalize controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_up) @@ -219,7 +294,12 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u stop_test_controller(test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -228,7 +308,12 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u // Configure from inactive state: controller can no be cleaned-up test_controller->simulate_cleanup_failure = true; EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(0u, cleanup_calls); } @@ -241,7 +326,12 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure start_test_controller(test_param.strictness); stop_test_controller(test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -253,7 +343,12 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure ControllerManagerRunner cm_runner(this); EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); } +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(1u, cleanup_calls); } @@ -278,7 +373,12 @@ TEST_P(SwitchTest, EmptyListOrNonExistentTest) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto params = GetParam(); auto result = std::get<0>(params); @@ -382,19 +482,36 @@ class TestTwoLoadedControllers : public TestLoadController, ASSERT_NE(controller_if2, nullptr); EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); ASSERT_EQ( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if1->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } }; TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) { cm_->configure_controller(CONTROLLER_NAME_1); +<<<<<<< HEAD EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); cm_->configure_controller(CONTROLLER_NAME_2); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); +======= + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + + cm_->configure_controller(CONTROLLER_NAME_2); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) @@ -406,9 +523,17 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Stop controller 1, start controller 2 // Both fail because controller 2 because it is not configured and STRICT is used @@ -423,9 +548,15 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { ControllerManagerRunner cm_runner(this); @@ -435,26 +566,52 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_1}, test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_2}, test_param.strictness); +<<<<<<< HEAD ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); +======= + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 3ee141edaa..2ddaf6d514 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -14,11 +14,17 @@ #include #include +<<<<<<< HEAD #include #include #include #include "controller_interface/controller_interface.hpp" +======= +#include +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -51,10 +57,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -69,10 +82,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Test starting the second controller when the first is running @@ -89,10 +109,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Test stopping controller #1 and starting controller #2 @@ -108,10 +135,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Test stopping controller #2 and starting controller #1 @@ -127,10 +161,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Test stopping both controllers when only controller #1 is running @@ -151,10 +192,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Test stopping both controllers when only controller #1 is running @@ -172,10 +220,17 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } { // Test starting both controllers at the same time @@ -193,9 +248,16 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, +<<<<<<< HEAD abstract_test_controller1.c->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_state().id()); +======= + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a21c3b3b2c..e1d9c2db60 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -19,7 +19,10 @@ #include #include +<<<<<<< HEAD #include "controller_interface/controller_interface.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -108,6 +111,7 @@ TEST_F(TestLoadController, spawner_test_type_in_param) EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); +<<<<<<< HEAD auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); @@ -123,14 +127,71 @@ TEST_F(TestLoadController, spawner_test_type_in_param) // We should be able to reconfigure and activate am unconfigured loaded controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + // Try to spawn again, it should fail because already active + EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active"; + + std::vector start_controllers = {}; + std::vector stop_controllers = {"ctrl_1"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // We should be able to reconfigure and activate a configured controller + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->unload_controller("ctrl_1"); + cm_->load_controller("ctrl_1"); + + // We should be able to reconfigure and activate am unconfigured loaded controller + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Unload and reload EXPECT_EQ(call_unspawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been loaded"; +<<<<<<< HEAD ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestLoadController, multi_ctrls_test_type_in_param) @@ -156,7 +217,12 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } }; @@ -241,7 +307,11 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); @@ -264,6 +334,7 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position"); } +<<<<<<< HEAD TEST_F(TestLoadController, spawner_test_type_in_arg) { ControllerManagerRunner cm_runner(this); @@ -281,6 +352,8 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(TestLoadController, spawner_test_type_in_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + @@ -301,7 +374,11 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0], @@ -314,7 +391,11 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD chain_ctrl_with_parameters_and_type.c->get_state().id(), +======= + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], @@ -332,7 +413,12 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ( cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); @@ -340,12 +426,91 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ( cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); } +<<<<<<< HEAD +======= +TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_wildcard_entries.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_1 wildcard_ctrl_2 wildcard_ctrl_3 -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0); + + auto verify_ctrl_parameter = [](const auto & ctrl_node, bool has_param_3) + { + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint1"})); + + if (!ctrl_node->has_parameter("param1")) + { + ctrl_node->declare_parameter("param1", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param1").as_double(), 1.0); + + if (!ctrl_node->has_parameter("param2")) + { + ctrl_node->declare_parameter("param2", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param2").as_double(), 2.0); + + if (!ctrl_node->has_parameter("param3")) + { + ctrl_node->declare_parameter("param3", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param3").as_double(), has_param_3 ? 3.0 : -10.0); + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + + auto wildcard_ctrl_3 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(wildcard_ctrl_3.info.name, "wildcard_ctrl_3"); + ASSERT_EQ(wildcard_ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + wildcard_ctrl_3.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true); + + auto wildcard_ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(wildcard_ctrl_2.info.name, "wildcard_ctrl_2"); + ASSERT_EQ(wildcard_ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + wildcard_ctrl_2.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_2.c->get_node(), false); + + auto wildcard_ctrl_1 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(wildcard_ctrl_1.info.name, "wildcard_ctrl_1"); + ASSERT_EQ(wildcard_ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + wildcard_ctrl_1.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill @@ -356,8 +521,12 @@ TEST_F(TestLoadController, unload_on_kill) ss << "timeout --signal=INT 5 " << std::string(coveragepy_script) + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " +<<<<<<< HEAD << "ctrl_3 -c test_controller_manager -t " << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill"; +======= + << "ctrl_3 -c test_controller_manager --unload-on-kill"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_NE(std::system(ss.str().c_str()), 0) << "timeout should have killed spawner and returned non 0 code"; @@ -384,6 +553,173 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +<<<<<<< HEAD +======= +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint10"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "impedance") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + +TEST_F(TestLoadController, spawner_test_fallback_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + 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)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); + } + + // Try to spawn now the controller with fallback controllers inside the yaml + EXPECT_EQ( + call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string_array()[0], test_file_path); + + auto ctrl_3 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); + ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); + ASSERT_EQ( + ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string_array()[0], test_file_path); + } +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(TestLoadController, spawner_with_many_controllers) { std::stringstream ss; @@ -406,7 +742,11 @@ TEST_F(TestLoadController, spawner_with_many_controllers) { auto ctrl = cm_->get_loaded_controllers()[i]; ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + ASSERT_EQ(ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } @@ -487,7 +827,12 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } @@ -497,7 +842,11 @@ class TestLoadControllerWithNamespacedCM public: TestLoadControllerWithNamespacedCM() : ControllerManagerFixture( +<<<<<<< HEAD ros2_control_test_assets::minimal_robot_urdf, false, "foo_namespace") +======= + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } @@ -563,7 +912,12 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } }; @@ -661,7 +1015,11 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") @@ -675,7 +1033,11 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD chain_ctrl_with_parameters_and_type.c->get_state().id(), +======= + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") @@ -694,14 +1056,24 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ( cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ( cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } @@ -749,7 +1121,11 @@ TEST_F( ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") @@ -763,7 +1139,11 @@ TEST_F( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD chain_ctrl_with_parameters_and_type.c->get_state().id(), +======= + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") @@ -783,14 +1163,24 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ( cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ( cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } @@ -824,7 +1214,11 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -835,7 +1229,11 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD chain_ctrl_with_parameters_and_type.c->get_state().id(), +======= + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -850,12 +1248,22 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F( @@ -896,7 +1304,11 @@ TEST_F( ASSERT_EQ( ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } @@ -904,9 +1316,15 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; +<<<<<<< HEAD const std::string basic_ctrls_test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_basic_controllers.yaml"; +======= + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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)); @@ -918,7 +1336,11 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " "--load-only -c " "test_controller_manager -p " + +<<<<<<< HEAD test_file_path + " -p" + basic_ctrls_test_file_path), +======= + test_file_path + " -p" + fallback_test_file_path), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); @@ -927,7 +1349,11 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto params_file_info = cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -941,7 +1367,11 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD chain_ctrl_with_parameters_and_type.c->get_state().id(), +======= + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -951,27 +1381,49 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) auto ctrl_2 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); +======= + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto ctrl_1 = cm_->get_loaded_controllers()[3]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; +<<<<<<< HEAD const std::string basic_ctrls_test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_basic_controllers.yaml"; +======= + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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)); @@ -983,8 +1435,13 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " "--load-only -c " "test_controller_manager -p " + +<<<<<<< HEAD test_file_path + " -p" + basic_ctrls_test_file_path + " -p" + basic_ctrls_test_file_path + " -p" + test_file_path), +======= + test_file_path + " -p" + fallback_test_file_path + " -p" + fallback_test_file_path + " -p" + + test_file_path), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); @@ -993,7 +1450,11 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD ctrl_with_parameters_and_type.c->get_state().id(), +======= + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto params_file_info = cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -1007,7 +1468,11 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( +<<<<<<< HEAD chain_ctrl_with_parameters_and_type.c->get_state().id(), +======= + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -1017,16 +1482,32 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) auto ctrl_2 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); +======= + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto ctrl_1 = cm_->get_loaded_controllers()[3]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); +======= + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 4393325a7c..9a4328f917 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -115,6 +116,166 @@ Changelog for package controller_manager_msgs 2.16.0 (2022-10-17) ------------------- +======= +4.21.0 (2024-12-06) +------------------- + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- + +4.18.0 (2024-10-07) +------------------- + +4.17.0 (2024-09-11) +------------------- + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + +4.13.0 (2024-07-08) +------------------- +* [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) +* Contributors: Sai Kishor Kothakota + +4.12.0 (2024-07-01) +------------------- + +4.11.0 (2024-05-14) +------------------- + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ + +4.8.0 (2024-03-27) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-03-02) +------------------ + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ +* Remove deprecations from CLI and controller_manager (`#948 `_) + Co-authored-by: Bence Magyar +* Contributors: Christoph Fröhlich + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Contributors: Bence Magyar + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2ba8f23194..2051f021ad 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(controller_manager_msgs) @@ -5,6 +6,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +======= +cmake_minimum_required(VERSION 3.16) +project(controller_manager_msgs) + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(lifecycle_msgs REQUIRED) diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 3b8e032541..14ae6ea1a3 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -6,5 +6,9 @@ string[] required_command_interfaces # command interfaces required by con string[] required_state_interfaces # state interfaces required by controller bool is_chainable # specifies whether or not controller can export references for a controller chain bool is_chained # specifies whether or not controller's exported references are claimed by another controller +<<<<<<< HEAD +======= +string[] exported_state_interfaces # state interfaces to be exported (only applicable if is_chainable is true) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) string[] reference_interfaces # references to be exported (only applicable if is_chainable is true) ChainConnection[] chain_connections # specifies list of controllers and their exported references that the controller is chained to diff --git a/controller_manager_msgs/msg/HardwareComponentState.msg b/controller_manager_msgs/msg/HardwareComponentState.msg index 51fd170701..ed01379d30 100644 --- a/controller_manager_msgs/msg/HardwareComponentState.msg +++ b/controller_manager_msgs/msg/HardwareComponentState.msg @@ -1,6 +1,11 @@ string name string type +<<<<<<< HEAD string class_type +======= +string class_type # DEPRECATED +string plugin_name +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs/State state HardwareInterface[] command_interfaces HardwareInterface[] state_interfaces diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index e800aeefb3..fdeecd732d 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,11 @@ controller_manager_msgs +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv index 29cbb7c1bd..9eb3007970 100644 --- a/controller_manager_msgs/srv/SwitchController.srv +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -21,12 +21,18 @@ string[] activate_controllers string[] deactivate_controllers +<<<<<<< HEAD string[] start_controllers # DEPRECATED: Use activate_controllers filed instead string[] stop_controllers # DEPRECATED: Use deactivate_controllers filed instead int32 strictness int32 BEST_EFFORT=1 int32 STRICT=2 bool start_asap # DEPRECATED: Use activate_asap filed instead +======= +int32 strictness +int32 BEST_EFFORT=1 +int32 STRICT=2 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) bool activate_asap builtin_interfaces/Duration timeout --- diff --git a/doc/debugging.rst b/doc/debugging.rst index 5349c98a52..6fe84f905c 100644 --- a/doc/debugging.rst +++ b/doc/debugging.rst @@ -65,10 +65,18 @@ Additional notes * Realtime +<<<<<<< HEAD .. warning:: The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). +======= + .. warning:: + + The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. + + From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) However, running the controller_manager and your plugin with gdb can still be very useful for debugging errors such as segfaults, as you can gather a full backtrace. diff --git a/doc/migration.rst b/doc/migration.rst index cb11ab2449..490fdeaa05 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -1,3 +1,4 @@ +<<<<<<< HEAD :github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/migration.rst Migration Guides: Galactic to Humble @@ -7,3 +8,169 @@ This list summarizes important changes between Galactic (previous) and Humble (c .. note:: This list was created in July 2024, earlier changes are not included. +======= +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst + +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +controller_interface +******************** +* The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. + In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments. +* 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. + +controller_manager +****************** +* Rename ``class_type`` to ``plugin_name`` (`#780 `_) +* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). As a consequence, when using multiple controller managers, you have to remap the topic within the launch file, an example for a python launch file: + + .. code-block:: python + + remappings=[ + ('/robot_description', '/custom_1/robot_description'), + ] + +* Changes from `(PR #1256) `__ + + * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: + + The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. + + This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. + + * The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of + + .. code-block:: xml + + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + define your mimic joints directly in the joint definitions: + + .. code-block:: xml + + + + + + + + + + + + + + + + +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). Use ``robot_description`` topic instead, e.g., you can use the `robot_state_publisher `_. For an example, see `this PR `_ where the change was applied to the demo repository. + +hardware_interface +****************** +* ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_) +* 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. + +Adaption of Command-/StateInterfaces +*************************************** + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which get parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). The memory is now allocated in the handle itself. + +Migration of Command-/StateInterfaces +------------------------------------- +To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: + +1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. +2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.: + + * If you have a ``std::vector hw_commands_;`` for joints' ``CommandInterfaces`` delete the definition and any usage/appearance. + * Wherever you iterated over a state/command or accessed commands like this: + +.. code-block:: c++ + + // states + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = 0; + auto some_state = hw_states_[i]; + } + + // commands + for (uint i = 0; i < hw_commands_.size(); i++) + { + hw_commands_[i] = 0; + auto some_command = hw_commands_[i]; + } + + // specific state/command + hw_commands_[x] = hw_states_[y]; + +replace it with + +.. code-block:: c++ + + // states replace with this + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + auto some_state = get_state(name); + } + + //commands replace with this + for (const auto & [name, descr] : joint_commands_interfaces_) + { + set_command(name, 0.0); + auto some_command = get_command(name); + } + + // replace specific state/command, for this you need to store the names which are strings + // somewhere or know them. However be careful since the names are fully qualified names which + // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity + set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); + +Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` XML-tag +-------------------------------------------------------------------------------------- +If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: + +1. Override the ``virtual std::vector export_unlisted_command_interfaces()`` or ``virtual std::vector export_unlisted_state_interfaces()`` +2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + +3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. +4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. + +Custom export of Command-/StateInterfaces +---------------------------------------------- +In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + +* If you want to have unlisted interfaces available you need to call the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. +* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``std::shared_ptr`` and the resource manager will not provide access to the created ``Command-/StateInterface`` for the hardware. So you must take care of storing them yourself. +* Names must be unique! +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9291a4dbc4..616ef9fa42 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -1,3 +1,4 @@ +<<<<<<< HEAD :github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst Release Notes: Galactic to Humble @@ -23,3 +24,197 @@ controller_manager * 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 `_). * ``--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 `_). * 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 `_). +======= +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst + +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +General +******* +* A ``version.h`` file will be generated per package using the ament_generate_version_header (`#1449 `_). For example, you can include the version of the package in the logs. + + .. code-block:: cpp + + #include + ... + RCLCPP_INFO(get_logger(), "controller_manager version: %s", CONTROLLER_MANAGER_VERSION_STR); + +controller_interface +******************** +For details see the controller_manager section. + +* Pass URDF to controllers on init (`#1088 `_). +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* A method to get node options to setup the controller node #api-breaking (`#1169 `_) +* Export state interfaces from the chainable controller #api-breaking (`#1021 `_) +* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* 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. +* The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) +* The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) +* The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) + +controller_manager +****************** +* URDF is now passed to controllers on init (`#1088 `_) + This should help avoiding extra legwork in controllers to get access to the ``/robot_description``. +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_) +* Set chained controller interfaces 'available' for activated controllers (`#1098 `_) + + * Configured chainable controller: Listed exported interfaces are unavailable and unclaimed + * Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed + * Active chainable controller (in chained mode): Listed exported interfaces are available and claimed +* Try using SCHED_FIFO on any kernel (`#1142 `_) +* A method to get node options to setup the controller node was added (`#1169 `_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options +* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). +* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_) +* Changes from `(PR #1256) `__ + + * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: + + The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. + + This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. + + * The syntax for mimic joints is changed to the `official URDF specification `__. + + .. code-block:: xml + + + + + + + + + + + + + + + + + + The parameters within the ``ros2_control`` tag are not supported any more. +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). +* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). +* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). +* Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* 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 `_). +* 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 `_). +* The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). +* The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). + +hardware_interface +****************** +* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) +* ``test_components`` was moved to its own package (`#1325 `_) +* The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + + +* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) +* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* The ``ros2_control`` tag now supports parsing read/write rate ``rw_rate`` for the each hardware component parsed through the URDF (`#1570 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + +* 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 +************ +* Add header to import limits from standard URDF definition (`#1298 `_) + +Adaption of Command-/StateInterfaces +*************************************** +Changes from `(PR #1688) `_ for an overview of related changes and discussion refer to `(PR #1240) `_. + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). +* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. +* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``. + + +ros2controlcli +************** +* Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 `_) +* The ``set_hardware_component_state`` verb was added (`#1248 `_). Use the following command to set the state of a hardware component + + .. code-block:: bash + + ros2 control set_hardware_component_state + +* The ``load_controller`` now supports parsing of the params file (`#1703 `_). + + .. code-block:: bash + + ros2 control load_controller + +* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 `_). + + .. code-block:: bash + + ros2 control --ros-args -r __ns:= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 51d2cca0ae..fb9d63bb53 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -186,6 +187,290 @@ Changelog for package hardware_interface ------------------- * fix broken links (issue `#831 `_) (`#833 `_) (`#845 `_) * Contributors: Manuel Muth +======= +4.21.0 (2024-12-06) +------------------- +* [Feature] Choose different read and write rate for the hardware components (`#1570 `_) +* add logic for 'params_file' to handle both string and string_array (`#1898 `_) +* [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) +* Fix missing virtual of on_export\_[state|command]_interfaces methods (`#1888 `_) +* Refactor: add parse_state_interface_descriptions and parse_command_interface_descriptions to import the components (`#1768 `_) +* Contributors: Sai Kishor Kothakota, Takashi Sato + +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 + +4.19.0 (2024-10-26) +------------------- +* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) +* Contributors: Sai Kishor Kothakota + +4.18.0 (2024-10-07) +------------------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) +* [RM] Execute `error` callback of component on returning ERROR or with exception (`#1730 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + +4.17.0 (2024-09-11) +------------------- +* Log exception type when catching the exception (`#1749 `_) +* Fix spam of logs on failed hardware component initialization (`#1719 `_) +* [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) +* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- +* Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) +* Introduce Creation of Handles with InterfaceDescription (variant support) (`#1679 `_) +* Preparation of Handles for Variant Support (`#1678 `_) +* [RM] Decouple read/write cycles of each component with mutex to not block other components (`#1646 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + +4.15.0 (2024-08-05) +------------------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* add missing rclcpp logging include for Humble compatibility build (`#1635 `_) +* Contributors: Sai Kishor Kothakota + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* [ResourceManager] Make destructor virtual for use in derived classes (`#1607 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.13.0 (2024-07-08) +------------------- +* [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) +* [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) +* Remove mimic parameter from ros2_control tag (`#1553 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.12.0 (2024-07-01) +------------------- +* Add resources_lock\_ lock_guards to avoid race condition when loading robot_description through topic (`#1451 `_) +* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 `_) +* Small improvements to the error output in component parser to make debugging easier. (`#1580 `_) +* Fix link to gazebosim.org (`#1563 `_) +* Add doc page about joint kinematics (`#1497 `_) +* Bump version of pre-commit hooks (`#1556 `_) +* [Feature] Hardware Components Grouping (`#1458 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Sai Kishor Kothakota, github-actions[bot] + +4.11.0 (2024-05-14) +------------------- +* Add find_package for ament_cmake_gen_version_h (`#1534 `_) +* Parse URDF soft_limits into the HardwareInfo structure (`#1488 `_) +* Contributors: Christoph Fröhlich, adriaroig + +4.10.0 (2024-05-08) +------------------- +* Add hardware components exception handling in resource manager (`#1508 `_) +* Working async controllers and components [not synchronized] (`#1041 `_) +* Parse URDF joint hard limits into the HardwareInfo structure (`#1472 `_) +* Add fallback controllers list to the ControllerInfo (`#1503 `_) +* Add more common hardware interface type constants (`#1500 `_) +* Contributors: Márk Szitanics, Sai Kishor Kothakota + +4.9.0 (2024-04-30) +------------------ +* Add missing calculate_dynamics (`#1498 `_) +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + +4.8.0 (2024-03-27) +------------------ +* generate version.h file per package using the ament_generate_version_header (`#1449 `_) +* Contributors: Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* Codeformat from new pre-commit config (`#1433 `_) +* Contributors: Christoph Fröhlich + +4.6.0 (2024-03-02) +------------------ +* Add -Werror=missing-braces to compile options (`#1423 `_) +* [CI] Code coverage + pre-commit (`#1413 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.5.0 (2024-02-12) +------------------ +* Add missing export macros in lexical_casts.hpp (`#1382 `_) +* Move hardware interface README content to sphinx documentation (`#1342 `_) +* [Doc] Add documentation about initial_value regarding mock_hw (`#1352 `_) +* Contributors: Felix Exner (fexner), Mateus Menezes, Silvio Traversaro + +4.4.0 (2024-01-31) +------------------ +* Move `test_components` to own package (`#1325 `_) +* Fix controller parameter loading issue in different cases (`#1293 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.3.0 (2024-01-20) +------------------ +* [RM] Fix crash for missing urdf in resource manager (`#1301 `_) +* Add additional checks for non existing and not available interfaces. (`#1218 `_) +* Adding backward compatibility for string-to-double conversion (`#1284 `_) +* [Doc] Make interface comments clearer in the doc strings. (`#1288 `_) +* Fix return of ERROR and calls of cleanup when system is unconfigured of finalized (`#1279 `_) +* fix the multiple definitions of lexical casts methods (`#1281 `_) +* [ResourceManager] adds test for uninitialized hardware (`#1243 `_) +* Use portable version for string-to-double conversion (`#1257 `_) +* Fix typo in docs (`#1219 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Maximilian Schik, Sai Kishor Kothakota, Stephanie Eng, bailaC + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ +* [MockHardware] Remove all deprecated options and deprecated plugins from the library. (`#1150 `_) +* Contributors: Dr. Denis + +3.21.0 (2023-11-06) +------------------- +* [MockHardware] Fix the issues where hardware with multiple interfaces can not be started because of a logical bug added when adding dynamics calculation functionality. (`#1151 `_) +* Fix potential deadlock in ResourceManager (`#925 `_) +* Contributors: Christopher Wecht, Dr. Denis + +3.20.0 (2023-10-31) +------------------- +* [ResourceManager] deactivate hardware from read/write return value (`#884 `_) +* Contributors: Felix Exner (fexner) + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- +* [MockHardware] Added dynamic simulation functionality. (`#1028 `_) +* Add GPIO tag description to docs (`#1109 `_) +* Contributors: Christoph Fröhlich, Dr. Denis + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- +* Add checks if hardware is initialized. (`#1054 `_) +* Contributors: Dr. Denis + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- +* Enable setting of initial state in HW compoments (`#1046 `_) +* Ensure instantiation of hardware classes work for python bindings (`#1058 `_) +* Contributors: Dr. Denis, Olivier Stasse + +3.14.0 (2023-06-14) +------------------- +* Add -Wconversion flag to protect future developments (`#1053 `_) +* [CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (`#940 `_) +* [MockHardware] Enable disabling of command to simulate HW failures. (`#1027 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#1037 `_) +* Docs: Use branch name substitution for all links (`#1031 `_) +* [URDF Parser] Allow empty urdf tag, e.g., parameter (`#1017 `_) +* Use consequently 'mock' instead of 'fake'. (`#1026 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota, gwalck + +3.13.0 (2023-05-18) +------------------- +* Add class for thread management of async hw interfaces (`#981 `_) +* Fix github links on control.ros.org (`#1019 `_) +* Update precommit libraries(`#1020 `_) +* Implement parse_bool and refactor a few (`#1014 `_) +* docs: Fix link to hardware_components (`#1009 `_) +* Contributors: Alejandro Bordallo, Christoph Fröhlich, Felix Exner (fexner), Márk Szitanics, mosfet80 + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- +* Check for missing hardware interfaces that use the gpio tag. (`#975 `_) +* Contributors: Ryan Sandzimier + +3.10.0 (2023-03-16) +------------------- +* Split transmission interfaces (`#938 `_) +* Contributors: Noel Jiménez García + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Async params (`#927 `_) +* Contributors: Márk Szitanics, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ +* Make double parsing locale independent (`#921 `_) +* Contributors: Henning Kayser + +3.6.0 (2023-01-12) +------------------ +* 🔧 Fixes and updated on pre-commit hooks and their action (`#890 `_) +* Contributors: Denis Štogl + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* ResourceManager doesn't always log an error on shutdown anymore (`#867 `_) +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Contributors: Bence Magyar, Christopher Wecht + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ +* [MockHardware] Enalbe initialization non-joint components(`#822 `_) +* Contributors: Felix Exner + +3.2.0 (2022-10-15) +------------------ +* [MockComponents] Rename 'fake_sensor_commands' to 'mock_sensor_commands' (`#782 `_) +* fix broken links (issue `#831 `_) (`#833 `_) +* Contributors: Kvk Praneeth, Manuel Muth, Bence Magyar, Denis Štogl + +3.1.0 (2022-10-05) +------------------ +* Cleanup Resource Manager a bit to increase clarity. (`#816 `_) +* Handle hardware errors in Resource Manager (`#805 `_) + * Add code for deactivating controller when hardware gets an error on read and write. +* Contributors: Denis Štogl + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 54474889f8..fb17c6e2c8 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(hardware_interface) @@ -17,13 +18,43 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +======= +cmake_minimum_required(VERSION 3.16) +project(hardware_interface LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Werror=missing-braces) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + lifecycle_msgs + pluginlib + rclcpp_lifecycle + rcpputils + rcutils + TinyXML2 + tinyxml2_vendor + joint_limits + urdf +) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_gen_version_h REQUIRED) +find_package(backward_ros REQUIRED) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +<<<<<<< HEAD add_library( ${PROJECT_NAME} SHARED +======= +add_library(hardware_interface SHARED +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) src/actuator.cpp src/component_parser.cpp src/resource_manager.cpp @@ -31,6 +62,7 @@ add_library( src/system.cpp src/lexical_casts.cpp ) +<<<<<<< HEAD target_include_directories( ${PROJECT_NAME} PUBLIC @@ -62,11 +94,33 @@ ament_target_dependencies( rcpputils ) +======= +target_compile_features(hardware_interface PUBLIC cxx_std_17) +target_include_directories(hardware_interface PUBLIC + $ + $ +) +ament_target_dependencies(hardware_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") + +add_library(mock_components SHARED + src/mock_components/generic_system.cpp +) +target_compile_features(mock_components PUBLIC cxx_std_17) +target_include_directories(mock_components PUBLIC + $ + $ +) +ament_target_dependencies(mock_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(mock_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") pluginlib_export_plugin_description_file( +<<<<<<< HEAD ${PROJECT_NAME} mock_components_plugin_description.xml) # Fake components @@ -113,6 +167,9 @@ install( ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) +======= + hardware_interface mock_components_plugin_description.xml) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if(BUILD_TESTING) @@ -128,6 +185,7 @@ if(BUILD_TESTING) ament_target_dependencies(test_inst_hardwares rcpputils) ament_add_gmock(test_joint_handle test/test_handle.cpp) +<<<<<<< HEAD target_link_libraries(test_joint_handle ${PROJECT_NAME}) ament_target_dependencies(test_joint_handle rcpputils) @@ -136,32 +194,65 @@ if(BUILD_TESTING) ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser ${PROJECT_NAME}) +======= + target_link_libraries(test_joint_handle hardware_interface) + ament_target_dependencies(test_joint_handle rcpputils) + + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) + target_link_libraries(test_component_interfaces hardware_interface) + ament_target_dependencies(test_component_interfaces ros2_control_test_assets) + + ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) + target_link_libraries(test_component_interfaces_custom_export hardware_interface) + ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + + ament_add_gmock(test_component_parser test/test_component_parser.cpp) + target_link_libraries(test_component_parser hardware_interface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_target_dependencies(test_component_parser ros2_control_test_assets) add_library(test_hardware_components SHARED test/test_hardware_components/test_single_joint_actuator.cpp test/test_hardware_components/test_force_torque_sensor.cpp +<<<<<<< HEAD test/test_hardware_components/test_two_joint_system.cpp test/test_hardware_components/test_system_with_command_modes.cpp ) target_link_libraries(test_hardware_components ${PROJECT_NAME}) +======= + test/test_hardware_components/test_imu_sensor.cpp + test/test_hardware_components/test_two_joint_system.cpp + test/test_hardware_components/test_system_with_command_modes.cpp + ) + target_link_libraries(test_hardware_components hardware_interface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_target_dependencies(test_hardware_components pluginlib) install(TARGETS test_hardware_components DESTINATION lib ) pluginlib_export_plugin_description_file( +<<<<<<< HEAD ${PROJECT_NAME} test/test_hardware_components/test_hardware_components.xml ) ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) target_include_directories(test_generic_system PRIVATE include) target_link_libraries(test_generic_system ${PROJECT_NAME}) +======= + hardware_interface test/test_hardware_components/test_hardware_components.xml + ) + + ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) + target_include_directories(test_generic_system PRIVATE include) + target_link_libraries(test_generic_system hardware_interface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_target_dependencies(test_generic_system pluginlib ros2_control_test_assets ) endif() +<<<<<<< HEAD ament_export_include_directories( include ) @@ -170,6 +261,23 @@ ament_export_libraries( mock_components ${PROJECT_NAME} ) +======= +install( + DIRECTORY include/ + DESTINATION include/hardware_interface +) +install( + TARGETS + mock_components + hardware_interface + EXPORT export_hardware_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_hardware_interface HAS_LIBRARY_TARGET) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() ament_generate_version_header(${PROJECT_NAME}) diff --git a/hardware_interface/doc/different_update_rates_userdoc.rst b/hardware_interface/doc/different_update_rates_userdoc.rst index 23f5e3564a..4f81e2130d 100644 --- a/hardware_interface/doc/different_update_rates_userdoc.rst +++ b/hardware_interface/doc/different_update_rates_userdoc.rst @@ -5,6 +5,7 @@ Different update rates for Hardware Components =============================================================================== +<<<<<<< HEAD In the following sections you can find some advice which will help you to implement Hardware Components with update rates different from the main control loop. @@ -167,3 +168,88 @@ implementations is to measure elapsed time since last pass: .. |step-1| replace:: step 1 .. |step-2| replace:: step 2 +======= +The ``ros2_control`` framework allows to run different hardware components at different update rates. This is useful when some of the hardware components needs to run at a different frequency than the traditional control loop frequency which is same as the one of the ``controller_manager``. It is very typical to have different components with different frequencies in a robotic system with different sensors or different components using different communication protocols. +This is useful when you have a hardware component that needs to run at a higher rate than the rest of the components. For example, you might have a sensor that needs to be read at 1000Hz, while the rest of the components can run at 500Hz, given that the control loop frequency of the ``controller_manager`` is higher than 1000Hz. The read/write rate can be defined easily by adding the parameter ``rw_rate`` to the ``ros2_control`` tag of the hardware component. + +Examples +***************************** +The following examples show how to use the different hardware interface types with different update frequencies in a ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +For a RRBot with Multimodal gripper and external sensor running at different rates: + +.. code-block:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + + +In the above example, the system hardware component that controls the joints of the RRBot is running at 500 Hz, the multimodal gripper is running at 200 Hz and the force torque sensor is running at 250 Hz. + +.. note:: + In the above example, the ``rw_rate`` parameter is set to 500 Hz, 200 Hz and 250 Hz for the system, actuator and sensor hardware components respectively. This parameter is optional and if not set, the default value of 0 will be used which means that the hardware component will run at the same rate as the ``controller_manager``. However, if the specified rate is higher than the ``controller_manager`` rate, the hardware component will then run at the rate of the ``controller_manager``. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index cd1e475b20..7b9a4f37eb 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -9,11 +9,51 @@ The ``ros2_control`` framework provides a set of hardware interface types that c a hardware component for a specific robot or device. The following sections describe the different hardware interface types and their usage. +<<<<<<< HEAD +======= +Overview +***************************** +Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``. +The definition can be found in the `ros2_control repository `_. +You can check the structs defined there to see what attributes are available for each of the xml tags. +A generic example which shows the structure is provided below. More specific examples can be found in the Example part below. + +.. code:: xml + + + + library_name/ClassName + + value + + + + + -1 + 1 + 0.0 + + 5 + + some_value + other_value + + + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Joints ***************************** ````-tag groups the interfaces associated with the joints of physical robots and actuators. They have command and state interfaces to set the goal values for hardware and read its current state. +<<<<<<< HEAD +======= +All joints defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `. + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster ` Sensors @@ -39,6 +79,15 @@ The ```` tag can be used as a child of all three types of hardware compone Because ports implemented as ````-tag are typically very application-specific, there exists no generic publisher within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example ` as part of the demo repository. +<<<<<<< HEAD +======= +Hardware Groups +***************************** +Hardware Component Groups serve as a critical organizational mechanism within complex systems, facilitating error handling and fault tolerance. By grouping related hardware components together, such as actuators within a manipulator, users can establish a unified framework for error detection and response. + +Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ros2_control configuration, the ```` tag within each ```` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system. + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Examples ***************************** The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF. @@ -150,3 +199,69 @@ They can be combined together within the different hardware component types (sys +<<<<<<< HEAD +======= + +4. Robot with multiple hardware components belonging to same group : ``Group1`` + + - RRBot System 1 and 2 + - Digital: Total 4 inputs and 2 outputs + - Analog: Total 2 inputs and 1 output + - Vacuum valve at the flange (on/off) + - Group: Group1 + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + Group1 + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + 3.1 + + + + + + + + + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + Group1 + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + + + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/doc/joints_userdoc.rst b/hardware_interface/doc/joints_userdoc.rst index fccb8e42cc..a6f4cfa622 100644 --- a/hardware_interface/doc/joints_userdoc.rst +++ b/hardware_interface/doc/joints_userdoc.rst @@ -86,7 +86,11 @@ From the officially released packages, the following packages are already using * :ref:`mock_components (generic system) ` * :ref:`gazebo_ros2_control ` +<<<<<<< HEAD * :ref:`ign_ros2_control ` +======= +* :ref:`gz_ros2_control ` +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) As the URDF specifies only the kinematics, the mimic tag has to be independent of the hardware interface type used in ros2_control. This means that we interpret this info in the following way: diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 9c9e30dd8b..00a359326d 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -19,9 +19,16 @@ For more information about hardware components check :ref:`detailed documentatio Features: +<<<<<<< HEAD - support for mimic joints - mirroring commands to states with and without offset - fake command interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) +======= + - support for mimic joints, which is parsed from the URDF (see the `URDF wiki `__) + - mirroring commands to states with and without offset + - fake command interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) + - fake gpio interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Parameters @@ -93,6 +100,7 @@ mock_sensor_commands (optional; boolean; default: false) position_state_following_offset (optional; double; default: 0.0) Following offset added to the commanded values when mirrored to states. Only applied, if ``custom_interface_with_following_offset`` is false. +<<<<<<< HEAD Per-joint Parameters ,,,,,,,,,,,,,,,,,,,, @@ -106,6 +114,10 @@ multiplier (optional; double; default: 1; used if mimic joint is defined) Per-interface Parameters ,,,,,,,,,,,,,,,,,,,,,,,, +======= +Per-Interface Parameters +######################## +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) initial_value (optional; double) Initial value of certain state interface directly after startup. Example: diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 698f6cf6e2..7a059554f3 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -8,7 +8,11 @@ Writing a Hardware Component In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface. +<<<<<<< HEAD 1. **Preparing package** +======= +#. **Preparing package** +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) If the package for the hardware interface does not exist, then create it first. The package should have ``ament_cmake`` as a build type. @@ -17,7 +21,11 @@ The following is a step-by-step guide to create source files, basic tests, and c Use the ``--help`` flag for more information on how to use it. There is also an option to create library source files and compile rules to help you in the following steps. +<<<<<<< HEAD 2. **Preparing source files** +======= +#. **Preparing source files** +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) After creating the package, you should have at least ``CMakeLists.txt`` and ``package.xml`` files in it. Create also ``include//`` and ``src`` folders if they do not already exist. @@ -25,7 +33,11 @@ The following is a step-by-step guide to create source files, basic tests, and c Optionally add ``visibility_control.h`` with the definition of export rules for Windows. You can copy this file from an existing controller package and change the name prefix to the ````. +<<<<<<< HEAD 3. **Adding declarations into header file (.hpp)** +======= +#. **Adding declarations into header file (.hpp)** +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). @@ -39,6 +51,7 @@ The following is a step-by-step guide to create source files, basic tests, and c class HardwareInterfaceName : public hardware_interface::$InterfaceType$Interface 5. Add a constructor without parameters and the following public methods implementing ``LifecycleNodeInterface``: ``on_configure``, ``on_cleanup``, ``on_shutdown``, ``on_activate``, ``on_deactivate``, ``on_error``; and overriding ``$InterfaceType$Interface`` definition: ``on_init``, ``export_state_interfaces``, ``export_command_interfaces``, ``prepare_command_mode_switch`` (optional), ``perform_command_mode_switch`` (optional), ``read``, ``write``. +<<<<<<< HEAD For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. 4. **Adding definitions into source file (.cpp)** @@ -72,20 +85,93 @@ The following is a step-by-step guide to create source files, basic tests, and c 13. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. 14. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. +======= + + For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. + +#. **Adding definitions into source file (.cpp)** + + #. Include the header file of your hardware interface and add a namespace definition to simplify further development. + + #. Implement ``on_init`` method. Here, you should initialize all member variables and process the parameters from the ``info`` argument. + In the first line usually the parents ``on_init`` is called to process standard values, like name. This is done using: ``hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)``. + If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise. + + #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. + + #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. + #. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). + + * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. + * For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. + * As a reminder, the full interface names have structure ``/``. + + #. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: + + #. Override the ``virtual std::vector export_unlisted_command_interface_descriptions()`` or ``virtual std::vector export_unlisted_state_interface_descriptions()`` + #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_unlisted_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + + #. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. + #. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. + + #. (optional) In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + + * If you want to have unlisted interfaces available you need to call the ``export_unlisted_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. + * Names must be unique! + + #. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. + + #. Implement the ``on_activate`` method where hardware "power" is enabled. + + #. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. + + #. Implement ``on_shutdown`` method where hardware is shutdown gracefully. + + #. Implement ``on_error`` method where different errors from all states are handled. + + #. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. + + #. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. + + #. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header. As first parameters you should provide exact hardware interface class, e.g., ``::``, and as second the base class, i.e., ``hardware_interface::(Actuator|Sensor|System)Interface``. +<<<<<<< HEAD 5. **Writing export definition for pluginlib** 1. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. The easiest way to do that is to check definition for mock components in the :ref:`hardware_interface mock_components ` section. 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., +======= +#. **Writing export definition for pluginlib** + + #. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. + The easiest way to do that is to check definition for mock components in the :ref:`hardware_interface mock_components ` section. + + #. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ``/``. This name defines the hardware interface's type when the resource manager searches for it. The other two parameters have to correspond to the definition done in the macro at the bottom of the ``.cpp`` file. +<<<<<<< HEAD 6. **Writing a simple test to check if the controller can be found and loaded** 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. @@ -106,10 +192,33 @@ The following is a step-by-step guide to create source files, basic tests, and c 4. Add ament dependencies needed by the library. You should add at least those listed under 1. 5. Export for pluginlib description file using the following command: +======= +#. **Writing a simple test to check if the controller can be found and loaded** + + #. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. + + #. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. + + #. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. + +#. **Add compile directives into ``CMakeLists.txt`` file** + + #. Under the line ``find_package(ament_cmake REQUIRED)`` add further dependencies. + Those are at least: ``hardware_interface``, ``pluginlib``, ``rclcpp`` and ``rclcpp_lifecycle``. + + #. Add a compile directive for a shared library providing the ``.cpp`` file as the source. + + #. Add targeted include directories for the library. This is usually only ``include``. + + #. Add ament dependencies needed by the library. You should add at least those listed under 1. + + #. Export for pluginlib description file using the following command: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .. code:: cmake pluginlib_export_plugin_description_file(hardware_interface .xml) +<<<<<<< HEAD 6. Add install directives for targets and include directory. 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. @@ -131,6 +240,29 @@ The following is a step-by-step guide to create source files, basic tests, and c Remember to go into the root of your workspace before executing this command. 2. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. +======= + #. Add install directives for targets and include directory. + + #. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. + + #. Add compile definitions for the tests using the ``ament_add_gmock`` directive. + For details, see how it is done for mock hardware in the `ros2_control `_ package. + + #. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. + +#. **Add dependencies into ``package.xml`` file** + + #. Add at least the following packages into ```` tag: ``hardware_interface``, ``pluginlib``, ``rclcpp``, and ``rclcpp_lifecycle``. + + #. Add at least the following package into ```` tag: ``ament_add_gmock`` and ``hardware_interface``. + +#. **Compiling and testing the hardware component** + + #. Now everything is ready to compile the hardware component using the ``colcon build `` command. + Remember to go into the root of your workspace before executing this command. + + #. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) That's it! Enjoy writing great controllers! diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 4082863370..ec482080e7 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -19,11 +19,20 @@ #include #include +<<<<<<< HEAD +======= +#include "hardware_interface/actuator_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" +<<<<<<< HEAD +======= +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -39,12 +48,23 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); +<<<<<<< HEAD Actuator(Actuator && other) = default; +======= + HARDWARE_INTERFACE_PUBLIC + explicit Actuator(Actuator && other) noexcept; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ~Actuator() = default; HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD const rclcpp_lifecycle::State & initialize(const HardwareInfo & actuator_info); +======= + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & actuator_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); @@ -65,10 +85,17 @@ class Actuator final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); +======= + std::vector export_state_interfaces(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interfaces(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( @@ -84,7 +111,20 @@ class Actuator final std::string get_name() const; HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD const rclcpp_lifecycle::State & get_state() const; +======= + std::string get_group_name() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & get_lifecycle_state() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_write_time() const; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -92,8 +132,21 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); +<<<<<<< HEAD +private: + std::unique_ptr impl_; +======= + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; + mutable std::recursive_mutex actuators_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1c77eb48ad..213cbaab9a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,16 +15,32 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ +<<<<<<< HEAD #include #include #include +======= +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/component_parser.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" +<<<<<<< HEAD +======= +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -73,7 +89,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod public: ActuatorInterface() : lifecycle_state_(rclcpp_lifecycle::State( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), + actuator_logger_(rclcpp::get_logger("actuator_interface")) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } @@ -88,6 +109,28 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual ~ActuatorInterface() = default; +<<<<<<< HEAD +======= + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock_interface pointer to the clock interface. + * \param[in] logger_interface pointer to the logger interface. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + { + clock_interface_ = clock_interface; + actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); + info_ = hardware_info; + return on_init(hardware_info); + }; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Initialization of the hardware interface from data parsed from the robot's URDF. /** * \param[in] hardware_info structure with data from URDF. @@ -97,18 +140,32 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; +<<<<<<< HEAD +======= + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return CallbackReturn::SUCCESS; }; /// Exports all state interfaces for this hardware interface. /** +<<<<<<< HEAD * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. +======= + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ +<<<<<<< HEAD virtual std::vector export_state_interfaces() = 0; /// Exports all command interfaces for this hardware interface. @@ -122,6 +179,150 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector export_command_interfaces() = 0; +======= + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_unlisted_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_state_interface_descriptions(); + + std::vector state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + actuator_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + actuator_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + return state_interfaces; + } + + /// Exports all command interfaces for this hardware interface. + /** + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. + * + * Note the ownership over the state interfaces is transferred to the caller. + * + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector on_export_command_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_command_interfaces() and only when empty vector is returned call + // on_export_command_interfaces() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector + export_unlisted_command_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + virtual std::vector on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_command_interface_descriptions(); + + std::vector command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + actuator_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + actuator_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + return command_interfaces; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -190,21 +391,104 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::string get_name() const { return info_.name; } +<<<<<<< HEAD +======= + /// Get name of the actuator hardware group to which it belongs to. + /** + * \return group name. + */ + virtual std::string get_group_name() const { return info_.group; } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Get life-cycle state of the actuator hardware. /** * \return state. */ +<<<<<<< HEAD const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } +======= + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Set life-cycle state of the actuator hardware. /** * \return state. */ +<<<<<<< HEAD void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; +======= + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } + + void set_state(const std::string & interface_name, const double & value) + { + actuator_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return actuator_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + actuator_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return actuator_commands_.at(interface_name)->get_value(); + } + + /// Get the logger of the ActuatorInterface. + /** + * \return logger of the ActuatorInterface. + */ + rclcpp::Logger get_logger() const { return actuator_logger_; } + + /// Get the clock of the ActuatorInterface. + /** + * \return clock of the ActuatorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + + /// Get the hardware info of the ActuatorInterface. + /** + * \return hardware info of the ActuatorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + +protected: + HardwareInfo info_; + // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; + + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; + + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; + std::vector joint_commands_; + + std::vector unlisted_states_; + std::vector unlisted_commands_; + + rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger actuator_logger_; + // interface names to Handle accessed through getters/setters + std::unordered_map actuator_states_; + std::unordered_map actuator_commands_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp new file mode 100644 index 0000000000..052c4ba921 --- /dev/null +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -0,0 +1,121 @@ +// Copyright 2023 ros2_control development team +// +// 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. + +#ifndef HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ +#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ + +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/time.hpp" + +namespace hardware_interface +{ + +class AsyncComponentThread +{ +public: + explicit AsyncComponentThread( + unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + : cm_update_rate_(update_rate), clock_interface_(clock_interface) + { + } + + // Fills the internal variant with the desired component. + template + void register_component(T * component) + { + hardware_component_ = component; + } + + AsyncComponentThread(const AsyncComponentThread & t) = delete; + AsyncComponentThread(AsyncComponentThread && t) = delete; + + // Destructor, called when the component is erased from its map. + ~AsyncComponentThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (write_and_read_.joinable()) + { + write_and_read_.join(); + } + } + /// Creates the component's thread. + /** + * Called when the component is activated. + * + */ + void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); } + + /// Periodically execute the component's write and read methods. + /** + * Callback of the async component's thread. + * **Not synchronized with the controller manager's update currently** + * + */ + void write_and_read() + { + using TimePoint = std::chrono::system_clock::time_point; + + std::visit( + [this](auto & component) + { + auto previous_time = clock_interface_->get_clock()->now(); + while (!terminated_.load(std::memory_order_relaxed)) + { + auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_); + TimePoint next_iteration_time = + TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds())); + + if ( + component->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto current_time = clock_interface_->get_clock()->now(); + auto measured_period = current_time - previous_time; + previous_time = current_time; + + if (!first_iteration) + { + component->write(clock_interface_->get_clock()->now(), measured_period); + } + component->read(clock_interface_->get_clock()->now(), measured_period); + first_iteration = false; + } + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); + } + }, + hardware_component_); + } + +private: + std::atomic terminated_{false}; + std::variant hardware_component_; + std::thread write_and_read_{}; + + unsigned int cm_update_rate_; + bool first_iteration = true; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; +}; + +}; // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index d5d999cca8..fd868fa008 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,5 +33,45 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +<<<<<<< HEAD +======= +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's StateInterfaces for the component + * which are exported + */ +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 + * which are exported + */ +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); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 3e25ca731e..38f451a95f 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -38,6 +38,12 @@ struct ControllerInfo /// List of claimed interfaces by the controller. std::vector claimed_interfaces; +<<<<<<< HEAD +======= + + /// List of fallback controller names to be activated if this controller fails. + std::vector fallback_controllers_names; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..04c542bad9 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +<<<<<<< HEAD #include #include @@ -53,16 +54,136 @@ class ReadOnlyHandle ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; virtual ~ReadOnlyHandle() = default; +======= +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/macros.hpp" + +namespace hardware_interface +{ + +using HANDLE_DATATYPE = std::variant; + +/// A handle used to get and set a value on a given interface. +class Handle +{ +public: + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + Handle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : prefix_name_(prefix_name), + interface_name_(interface_name), + handle_name_(prefix_name_ + "/" + interface_name_), + value_ptr_(value_ptr) + { + } + + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.get_prefix_name()), + interface_name_(interface_description.get_interface_name()), + handle_name_(interface_description.get_name()) + { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + explicit Handle(const std::string & interface_name) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) + { + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + explicit Handle(const char * interface_name) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) + { + } + + 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) 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) + { + 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) + { + 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; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } +<<<<<<< HEAD const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } +======= + const std::string & get_name() const { return handle_name_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const std::string & get_interface_name() const { return interface_name_; } [[deprecated( +<<<<<<< HEAD "Replaced by get_name method, which is semantically more correct")]] const std::string +======= + "Replaced by get_name method, which is semantically more correct")]] const std::string & +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) get_full_name() const { return get_name(); @@ -70,15 +191,62 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } +<<<<<<< HEAD + double get_value() const + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; +======= + [[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_); return *value_ptr_; + // END + } + + [[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 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } protected: std::string prefix_name_; std::string interface_name_; +<<<<<<< HEAD double * value_ptr_; }; @@ -116,16 +284,51 @@ class ReadWriteHandle : public ReadOnlyHandle class StateInterface : public ReadOnlyHandle { public: +======= + std::string handle_name_; + HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility + // 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 +{ +public: + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) + { + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; +<<<<<<< HEAD using ReadOnlyHandle::ReadOnlyHandle; }; class CommandInterface : public ReadWriteHandle { public: +======= + using Handle::Handle; + + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; +}; + +class CommandInterface : public Handle +{ +public: + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) + { + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -136,7 +339,13 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; +<<<<<<< HEAD using ReadWriteHandle::ReadWriteHandle; +======= + using Handle::Handle; + + using SharedPtr = std::shared_ptr; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 71dbfebade..f11332a4a7 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -19,10 +19,17 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ +<<<<<<< HEAD #include #include #include +======= +#include +#include + +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp_lifecycle/state.hpp" namespace hardware_interface @@ -39,8 +46,22 @@ struct HardwareComponentInfo /// Component "classification": "actuator", "sensor" or "system" std::string type; +<<<<<<< HEAD /// Component class type. std::string class_type; +======= + /// Component group + std::string group; + + /// Component pluginlib plugin name. + std::string plugin_name; + + /// Component is async + bool is_async; + + //// read/write rate + unsigned int rw_rate; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 85dd8febcf..62ff293332 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -19,6 +19,11 @@ #include #include +<<<<<<< HEAD +======= +#include "joint_limits/joint_limits.hpp" + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace hardware_interface { /** @@ -38,10 +43,40 @@ struct InterfaceInfo std::string max; /// (Optional) Initial value of the interface. std::string initial_value; +<<<<<<< HEAD /// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs. std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; +======= + /// (Optional) The datatype of the interface, e.g. "bool", "int". + std::string data_type; + /// (Optional) If the handle is an array, the size of the array. + int size; + /// (Optional) enable or disable the limits for the command interfaces + bool enable_limits; + /// (Optional) Key-value pairs of command/stateInterface parameters. This is + /// useful for drivers that operate on protocols like modbus, where each + /// interface needs own address(register), datatype, etc. + std::unordered_map parameters; +}; + +/// @brief This structure stores information about a joint that is mimicking another joint +struct MimicJoint +{ + std::size_t joint_index; + std::size_t mimicked_joint_index; + double multiplier = 1.0; + double offset = 0.0; +}; + +/// @brief This enum is used to store the mimic attribute of a joint +enum class MimicAttribute +{ + NOT_SET, + TRUE, + FALSE +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; /** @@ -54,6 +89,13 @@ struct ComponentInfo std::string name; /// Type of the component: sensor, joint, or GPIO. std::string type; +<<<<<<< HEAD +======= + + /// Hold the value of the mimic attribute if given, NOT_SET otherwise + MimicAttribute is_mimic = MimicAttribute::NOT_SET; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -101,6 +143,44 @@ struct TransmissionInfo std::unordered_map parameters; }; +<<<<<<< HEAD +======= +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), + interface_info(interface_info_in), + interface_name(prefix_name + "/" + interface_info.name) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ + InterfaceInfo interface_info; + + /** + * Name of the interface + */ + std::string interface_name; + + std::string get_prefix_name() const { return prefix_name; } + + std::string get_interface_name() const { return interface_info.name; } + + std::string get_name() const { return interface_name; } +}; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { @@ -108,27 +188,58 @@ struct HardwareInfo std::string name; /// Type of the hardware: actuator, sensor or system. std::string type; +<<<<<<< HEAD /// Class of the hardware that will be dynamically loaded. std::string hardware_class_type; /// (Optional) Key-value pairs for hardware parameters. std::unordered_map hardware_parameters; /** * Map of joints provided by the hardware where the key is the joint name. +======= + /// Hardware group to which the hardware belongs. + std::string group; + /// Component's read and write rates in Hz. + unsigned int rw_rate; + /// Component is async + bool is_async; + /// Name of the pluginlib plugin of the hardware that will be loaded. + std::string hardware_plugin_name; + /// (Optional) Key-value pairs for hardware parameters. + std::unordered_map hardware_parameters; + /** + * Vector of joints provided by the hardware. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * Required for Actuator and System Hardware. */ std::vector joints; /** +<<<<<<< HEAD * Map of sensors provided by the hardware where the key is the joint or link name. +======= + * Vector of mimic joints. + */ + std::vector mimic_joints; + /** + * Vector of sensors provided by the hardware. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * Required for Sensor and optional for System Hardware. */ std::vector sensors; /** +<<<<<<< HEAD * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO. +======= + * Vector of GPIOs provided by the hardware. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * Optional for any hardware components. */ std::vector gpios; /** +<<<<<<< HEAD * Map of transmissions to calculate ration between joints and physical actuators. +======= + * Vector of transmissions to calculate ratio between joints and physical actuators. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * Optional for Actuator and System Hardware. */ std::vector transmissions; @@ -136,6 +247,20 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; +<<<<<<< HEAD +======= + /** + * The URDF parsed limits of the hardware components joint command interfaces + */ + std::unordered_map limits; + + /** + * Map of software joint limits used for clamping the command where the key is the joint name. + * Optional If not specified or less restrictive than the JointLimits uses the previous + * JointLimits. + */ + std::unordered_map soft_limits; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp index 1b9bad7018..ee85c72552 100644 --- a/hardware_interface/include/hardware_interface/lexical_casts.hpp +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -15,11 +15,18 @@ #ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ #define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ +<<<<<<< HEAD #include #include #include #include +======= +#include + +#include "hardware_interface/visibility_control.h" + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace hardware_interface { @@ -28,8 +35,15 @@ namespace hardware_interface * from https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 */ +<<<<<<< HEAD +double stod(const std::string & s); + +======= +HARDWARE_INTERFACE_PUBLIC double stod(const std::string & s); +HARDWARE_INTERFACE_PUBLIC +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) bool parse_bool(const std::string & bool_string); } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp b/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp new file mode 100644 index 0000000000..1358da3058 --- /dev/null +++ b/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp @@ -0,0 +1,30 @@ +// Copyright 2024 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. + +#ifndef HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_ +#define HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_ + +#include + +namespace hardware_interface +{ +constexpr bool lifecycleStateThatRequiresNoAction(const lifecycle_msgs::msg::State::_id_type state) +{ + return state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN || + state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + state == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED; +} +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..a33b72ea31 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,10 +16,20 @@ #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ #include +<<<<<<< HEAD #include #include #include "hardware_interface/handle.hpp" +======= +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace hardware_interface { @@ -28,22 +38,64 @@ class LoanedCommandInterface public: using Deleter = std::function; +<<<<<<< HEAD explicit LoanedCommandInterface(CommandInterface & command_interface) +======= + [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedCommandInterface( + CommandInterface & command_interface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) : LoanedCommandInterface(command_interface, nullptr) { } +<<<<<<< HEAD LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) +======= + [[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface( + CommandInterface & command_interface, Deleter && deleter) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) : command_interface_(command_interface), deleter_(std::forward(deleter)) { } +<<<<<<< HEAD +======= + LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) + : command_interface_(*command_interface), deleter_(std::forward(deleter)) + { + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) LoanedCommandInterface(const LoanedCommandInterface & other) = delete; LoanedCommandInterface(LoanedCommandInterface && other) = default; virtual ~LoanedCommandInterface() { +<<<<<<< HEAD +======= + 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); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (deleter_) { deleter_(); @@ -63,13 +115,79 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } +<<<<<<< HEAD void set_value(double val) { command_interface_.set_value(val); } double get_value() const { return command_interface_.get_value(); } +======= + 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 + { + 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; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) protected: CommandInterface & command_interface_; Deleter deleter_; +<<<<<<< HEAD +======= + +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_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // 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 4fe67d1290..ae64645195 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,11 +16,21 @@ #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ #include +<<<<<<< HEAD #include #include #include "hardware_interface/handle.hpp" +======= +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace hardware_interface { class LoanedStateInterface @@ -28,22 +38,59 @@ class LoanedStateInterface public: using Deleter = std::function; +<<<<<<< HEAD explicit LoanedStateInterface(StateInterface & state_interface) +======= + [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedStateInterface( + const StateInterface & state_interface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) : LoanedStateInterface(state_interface, nullptr) { } +<<<<<<< HEAD LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) +======= + [[deprecated("Replaced by the new version using shared_ptr")]] LoanedStateInterface( + const StateInterface & state_interface, Deleter && deleter) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } +<<<<<<< HEAD +======= + explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface) + : LoanedStateInterface(state_interface, nullptr) + { + } + + LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter) + : state_interface_(*state_interface), deleter_(std::forward(deleter)) + { + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) LoanedStateInterface(const LoanedStateInterface & other) = delete; LoanedStateInterface(LoanedStateInterface && other) = default; virtual ~LoanedStateInterface() { +<<<<<<< HEAD +======= + 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); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (deleter_) { deleter_(); @@ -63,11 +110,58 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } +<<<<<<< HEAD double get_value() const { return state_interface_.get_value(); } protected: StateInterface & state_interface_; Deleter deleter_; +======= + 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_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 46723b3f72..a3ac9a1917 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -16,25 +16,46 @@ #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include +<<<<<<< HEAD +======= +#include "hardware_interface/actuator.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/duration.hpp" +======= +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" namespace hardware_interface { +<<<<<<< HEAD class ActuatorInterface; class SensorInterface; class SystemInterface; class ResourceStorage; +======= +class ResourceStorage; +class ControllerManager; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) struct HardwareReadWriteStatus { @@ -46,7 +67,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: /// Default constructor for the Resource Manager. +<<<<<<< HEAD ResourceManager(); +======= + explicit ResourceManager( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Constructor for the Resource Manager. /** @@ -54,6 +81,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * hardware components listed within as well as populate their respective * state and command interfaces. * +<<<<<<< HEAD * If the interfaces ought to be validated, the constructor throws an exception * in case the URDF lists interfaces which are not available. * @@ -73,10 +101,34 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /// Load resources from on a given URDF. /** * The resource manager can be post initialized with a given URDF. +======= + * \param[in] urdf string containing the URDF. + * \param[in] activate_all boolean argument indicating if all resources should be immediately + * activated. Currently used only in tests. + * \param[in] update_rate Update rate of the controller manager to calculate calling frequency + * of async components. + * \param[in] clock_interface reference to the clock interface of the CM node for getting time + * used for triggering async components. + */ + explicit ResourceManager( + const std::string & urdf, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, + bool activate_all = false, const unsigned int update_rate = 100); + + ResourceManager(const ResourceManager &) = delete; + + virtual ~ResourceManager(); + + /// Load resources from on a given URDF. + /** + * The resource manager can be post-initialized with a given URDF. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * This is mainly used in conjunction with the default constructor * in which the URDF might not be present at first initialization. * * \param[in] urdf string containing the URDF. +<<<<<<< HEAD * \param[in] validate_interfaces boolean argument indicating whether the exported * interfaces ought to be validated. Defaults to true. * \param[in] load_and_initialize_components boolean argument indicating whether to load and @@ -95,6 +147,23 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * @return false if resource manager's load_urdf() has not been yet called. */ bool is_urdf_already_loaded() const; +======= + * \param[in] update_rate update rate of the main control loop, i.e., of the controller manager. + * \returns false if URDF validation has failed. + */ + virtual bool load_and_initialize_components( + const std::string & urdf, const unsigned int update_rate = 100); + + /** + * @brief if the resource manager load_and_initialize_components(...) function has been called + * this returns true. We want to permit to loading the urdf later on, but we currently don't want + * to permit multiple calls to load_and_initialize_components (reloading/loading different urdf). + * + * @return true if the resource manager has successfully loaded and initialized the components + * @return false if the resource manager doesn't have any components loaded and initialized. + */ + bool are_components_initialized() const; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Claim a state interface given its key. /** @@ -127,6 +196,62 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; +<<<<<<< HEAD +======= + /// Add controllers' exported state interfaces to resource manager. + /** + * Interface for transferring management of exported state interfaces to resource manager. + * When chaining controllers, state interfaces are used by the preceding + * controllers. + * Therefore, they should be managed in the same way as state interface of hardware. + * + * \param[in] controller_name name of the controller which state interfaces are imported. + * \param[in] interfaces list of controller's state interfaces as StateInterfaces. + */ + void import_controller_exported_state_interfaces( + const std::string & controller_name, std::vector & interfaces); + + /// Get list of exported tate interface of a controller. + /** + * Returns lists of stored exported state interfaces names for a controller. + * + * \param[in] controller_name for which list of state interface names is returned. + * \returns list of reference interface names. + */ + std::vector get_controller_exported_state_interface_names( + const std::string & controller_name); + + /// Add controller's exported state interfaces to available list. + /** + * Adds state interfacess of a controller with given name to the available list. This method + * should be called when a controller gets activated with chained mode turned on. That means, the + * controller's exported state interfaces can be used by another controllers in chained + * architectures. + * + * \param[in] controller_name name of the controller which interfaces should become available. + */ + void make_controller_exported_state_interfaces_available(const std::string & controller_name); + + /// Remove controller's exported state interface to available list. + /** + * Removes interfaces of a controller with given name from the available list. This method should + * be called when a controller gets deactivated and its reference interfaces cannot be used by + * another controller anymore. + * + * \param[in] controller_name name of the controller which interfaces should become unavailable. + */ + void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name); + + /// Remove controllers exported state interfaces from resource manager. + /** + * Remove exported state interfaces from resource manager, i.e., resource storage. + * The interfaces will be deleted from all internal maps and lists. + * + * \param[in] controller_name list of interface names that will be deleted from resource manager. + */ + void remove_controller_exported_state_interfaces(const std::string & controller_name); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. @@ -138,7 +263,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. */ void import_controller_reference_interfaces( +<<<<<<< HEAD const std::string & controller_name, std::vector & interfaces); +======= + const std::string & controller_name, + const std::vector & interfaces); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Get list of reference interface of a controller. /** @@ -369,6 +499,15 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); +<<<<<<< HEAD +======= + /// Deletes all async components from the resource storage + /** + * Needed to join the threads immediately after the control loop is ended. + */ + void shutdown_async_components(); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Reads all loaded hardware components. /** * Reads from all active hardware components. @@ -387,6 +526,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period); +<<<<<<< HEAD /// Activates all available hardware components in the system. /** * All available hardware components int the ros2_control framework are activated. @@ -399,6 +539,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager "components")]] void activate_all_components(); +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Checks whether a command interface is registered under the given key. /** * \param[in] key string identifying the interface to check. @@ -412,23 +554,53 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_exists(const std::string & key) const; +<<<<<<< HEAD private: void validate_storage(const std::vector & hardware_info) const; void release_command_interface(const std::string & key); std::unordered_map claimed_command_interface_map_; +======= +protected: + /// Gets the logger for the resource manager + /** + * \return logger of the resource manager + */ + rclcpp::Logger get_logger() const; + + /// Gets the clock for the resource manager + /** + * \return clock of the resource manager + */ + rclcpp::Clock::SharedPtr get_clock() const; + + bool components_are_loaded_and_initialized_ = false; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; mutable std::recursive_mutex resources_lock_; +<<<<<<< HEAD +======= +private: + bool validate_storage(const std::vector & hardware_info) const; + + void release_command_interface(const std::string & key); + + std::unordered_map claimed_command_interface_map_; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::unique_ptr resource_storage_; // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; +<<<<<<< HEAD bool is_urdf_loaded__ = false; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index aaf9fcef8b..25976dbaaf 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -17,14 +17,26 @@ #include #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" +======= +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -40,12 +52,23 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); +<<<<<<< HEAD Sensor(Sensor && other) = default; +======= + HARDWARE_INTERFACE_PUBLIC + explicit Sensor(Sensor && other) noexcept; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ~Sensor() = default; HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD const rclcpp_lifecycle::State & initialize(const HardwareInfo & sensor_info); +======= + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & sensor_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); @@ -66,19 +89,47 @@ class Sensor final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD std::vector export_state_interfaces(); +======= + std::vector export_state_interfaces(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC std::string get_name() const; HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD const rclcpp_lifecycle::State & get_state() const; +======= + std::string get_group_name() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & get_lifecycle_state() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); +<<<<<<< HEAD +private: + std::unique_ptr impl_; +======= + HARDWARE_INTERFACE_PUBLIC + return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } + + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; + mutable std::recursive_mutex sensors_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 14a59e4588..1e110d26e7 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,16 +15,32 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ +<<<<<<< HEAD #include #include #include +======= +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/component_parser.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" +<<<<<<< HEAD +======= +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -73,7 +89,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI public: SensorInterface() : lifecycle_state_(rclcpp_lifecycle::State( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), + sensor_logger_(rclcpp::get_logger("sensor_interface")) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } @@ -88,6 +109,28 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual ~SensorInterface() = default; +<<<<<<< HEAD +======= + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock_interface pointer to the clock interface. + * \param[in] logger_interface pointer to the logger interface. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + { + clock_interface_ = clock_interface; + sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); + info_ = hardware_info; + return on_init(hardware_info); + }; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Initialization of the hardware interface from data parsed from the robot's URDF. /** * \param[in] hardware_info structure with data from URDF. @@ -97,19 +140,108 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; +<<<<<<< HEAD +======= + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return CallbackReturn::SUCCESS; }; /// Exports all state interfaces for this hardware interface. /** +<<<<<<< HEAD * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. +======= + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ +<<<<<<< HEAD virtual std::vector export_state_interfaces() = 0; +======= + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_unlisted_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the sensor_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_state_interface_descriptions(); + + std::vector state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + sensor_state_interfaces_.size() + + joint_state_interfaces_.size()); + + // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + for (const auto & [name, descr] : sensor_state_interfaces_) + { + // TODO(Manuel) check for duplicates otherwise only the first appearance of "name" is inserted + auto state_interface = std::make_shared(descr); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + return state_interfaces; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Read the current state values from the actuator. /** @@ -129,21 +261,89 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::string get_name() const { return info_.name; } +<<<<<<< HEAD +======= + /// Get name of the actuator hardware group to which it belongs to. + /** + * \return group name. + */ + virtual std::string get_group_name() const { return info_.group; } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Get life-cycle state of the actuator hardware. /** * \return state. */ +<<<<<<< HEAD const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } +======= + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Set life-cycle state of the actuator hardware. /** * \return state. */ +<<<<<<< HEAD void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; +======= + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } + + void set_state(const std::string & interface_name, const double & value) + { + sensor_states_map_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return sensor_states_map_.at(interface_name)->get_value(); + } + + /// Get the logger of the SensorInterface. + /** + * \return logger of the SensorInterface. + */ + rclcpp::Logger get_logger() const { return sensor_logger_; } + + /// Get the clock of the SensorInterface. + /** + * \return clock of the SensorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + + /// Get the hardware info of the SensorInterface. + /** + * \return hardware info of the SensorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + +protected: + HardwareInfo info_; + // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; + std::unordered_map sensor_state_interfaces_; + std::unordered_map unlisted_state_interfaces_; + + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; + std::vector sensor_states_; + std::vector unlisted_states_; + + rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger sensor_logger_; + // interface names to Handle accessed through getters/setters + std::unordered_map sensor_states_map_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index ece14f814d..c153621335 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -17,14 +17,26 @@ #include #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" +======= +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -40,12 +52,23 @@ class System final HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); +<<<<<<< HEAD System(System && other) = default; +======= + HARDWARE_INTERFACE_PUBLIC + explicit System(System && other) noexcept; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ~System() = default; HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD const rclcpp_lifecycle::State & initialize(const HardwareInfo & system_info); +======= + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & system_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); @@ -66,10 +89,17 @@ class System final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); +======= + std::vector export_state_interfaces(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interfaces(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( @@ -85,7 +115,20 @@ class System final std::string get_name() const; HARDWARE_INTERFACE_PUBLIC +<<<<<<< HEAD const rclcpp_lifecycle::State & get_state() const; +======= + std::string get_group_name() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & get_lifecycle_state() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_write_time() const; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -93,8 +136,21 @@ class System final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); +<<<<<<< HEAD +private: + std::unique_ptr impl_; +======= + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; + mutable std::recursive_mutex system_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 368778b171..acae658f77 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +<<<<<<< HEAD #include #include #include @@ -25,6 +26,26 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" +======= +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -74,7 +95,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI public: SystemInterface() : lifecycle_state_(rclcpp_lifecycle::State( +<<<<<<< HEAD lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) +======= + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), + system_logger_(rclcpp::get_logger("system_interface")) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } @@ -89,6 +115,28 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual ~SystemInterface() = default; +<<<<<<< HEAD +======= + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock_interface pointer to the clock interface. + * \param[in] logger_interface pointer to the logger interface. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + { + clock_interface_ = clock_interface; + system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); + info_ = hardware_info; + return on_init(hardware_info); + }; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Initialization of the hardware interface from data parsed from the robot's URDF. /** * \param[in] hardware_info structure with data from URDF. @@ -98,18 +146,35 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; +<<<<<<< HEAD +======= + 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_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return CallbackReturn::SUCCESS; }; /// Exports all state interfaces for this hardware interface. /** +<<<<<<< HEAD * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. +======= + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ +<<<<<<< HEAD virtual std::vector export_state_interfaces() = 0; /// Exports all command interfaces for this hardware interface. @@ -122,6 +187,172 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of command interfaces */ virtual std::vector export_command_interfaces() = 0; +======= + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_unlisted_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_state_interface_descriptions(); + + std::vector state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + system_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + for (const auto & [name, descr] : sensor_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + for (const auto & [name, descr] : gpio_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + gpio_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + return state_interfaces; + } + + /// Exports all command interfaces for this hardware interface. + /** + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. + * + * Note the ownership over the state interfaces is transferred to the caller. + * + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector on_export_command_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_command_interfaces() and only when empty vector is returned call + // on_export_command_interfaces() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector + export_unlisted_command_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + virtual std::vector on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_command_interface_descriptions(); + + std::vector command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + + gpio_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + system_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + system_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + system_commands_.insert(std::make_pair(name, command_interface)); + gpio_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + return command_interfaces; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Prepare for a new command interface switch. /** @@ -191,21 +422,114 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::string get_name() const { return info_.name; } +<<<<<<< HEAD +======= + /// Get name of the actuator hardware group to which it belongs to. + /** + * \return group name. + */ + virtual std::string get_group_name() const { return info_.group; } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Get life-cycle state of the actuator hardware. /** * \return state. */ +<<<<<<< HEAD const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } +======= + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Set life-cycle state of the actuator hardware. /** * \return state. */ +<<<<<<< HEAD void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; +======= + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } + + void set_state(const std::string & interface_name, const double & value) + { + system_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return system_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + system_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return system_commands_.at(interface_name)->get_value(); + } + + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return system_logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + + /// Get the hardware info of the SystemInterface. + /** + * \return hardware info of the SystemInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + +protected: + HardwareInfo info_; + // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; + + std::unordered_map sensor_state_interfaces_; + + std::unordered_map gpio_state_interfaces_; + std::unordered_map gpio_command_interfaces_; + + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; + + rclcpp_lifecycle::State lifecycle_state_; + + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; + std::vector joint_commands_; + + std::vector sensor_states_; + + std::vector gpio_states_; + std::vector gpio_commands_; + + std::vector unlisted_states_; + std::vector unlisted_commands_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger system_logger_; + // interface names to Handle accessed through getters/setters + std::unordered_map system_states_; + std::unordered_map system_commands_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 5c3ea22ca0..ca5753d506 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,6 +23,10 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, +<<<<<<< HEAD +======= + DEACTIVATE = 2, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index 98ca67226f..8b9dcb9f33 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -17,6 +17,7 @@ namespace hardware_interface { +<<<<<<< HEAD /// Constant defining position interface constexpr char HW_IF_POSITION[] = "position"; /// Constant defining velocity interface @@ -25,6 +26,38 @@ constexpr char HW_IF_VELOCITY[] = "velocity"; constexpr char HW_IF_ACCELERATION[] = "acceleration"; /// Constant defining effort interface constexpr char HW_IF_EFFORT[] = "effort"; +======= +/// Constant defining position interface name +constexpr char HW_IF_POSITION[] = "position"; +/// Constant defining velocity interface name +constexpr char HW_IF_VELOCITY[] = "velocity"; +/// Constant defining acceleration interface name +constexpr char HW_IF_ACCELERATION[] = "acceleration"; +/// Constant defining effort interface name +constexpr char HW_IF_EFFORT[] = "effort"; +/// Constant defining torque interface name +constexpr char HW_IF_TORQUE[] = "torque"; +/// Constant defining force interface name +constexpr char HW_IF_FORCE[] = "force"; +/// Constant defining current interface name +constexpr char HW_IF_CURRENT[] = "current"; +/// Constant defining temperature interface name +constexpr char HW_IF_TEMPERATURE[] = "temperature"; + +/// Gains interface constants +/// Constant defining proportional gain interface name +constexpr char HW_IF_PROPORTIONAL_GAIN[] = "proportional"; +/// Constant defining integral gain interface name +constexpr char HW_IF_INTEGRAL_GAIN[] = "integral"; +/// Constant defining derivative gain interface name +constexpr char HW_IF_DERIVATIVE_GAIN[] = "derivative"; +/// Constant defining integral clamp interface name +constexpr char HW_IF_INTEGRAL_CLAMP_MAX[] = "integral_clamp_max"; +/// Constant defining integral clamp interface name +constexpr char HW_IF_INTEGRAL_CLAMP_MIN[] = "integral_clamp_min"; +/// Constant defining the feedforward interface name +constexpr char HW_IF_FEEDFORWARD[] = "feedforward"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp b/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp index f51f64f836..6999fecd44 100644 --- a/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp +++ b/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp @@ -17,8 +17,11 @@ #ifndef HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ #define HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace hardware_interface { namespace lifecycle_state_names diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65d..6e12dbba8d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -25,6 +25,10 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +<<<<<<< HEAD +======= +#include "hardware_interface/visibility_control.h" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using hardware_interface::return_type; @@ -72,6 +76,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; +<<<<<<< HEAD struct MimicJoint { std::size_t joint_index; @@ -80,6 +85,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste }; std::vector mimic_joints_; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index f971de3731..5a687c7c6c 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,11 @@ hardware_interface +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ros2_control hardware interface Bence Magyar Denis Štogl @@ -10,12 +14,22 @@ ament_cmake ament_cmake_gen_version_h +<<<<<<< HEAD +======= + backward_ros +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) control_msgs lifecycle_msgs pluginlib rclcpp_lifecycle rcpputils tinyxml2_vendor +<<<<<<< HEAD +======= + joint_limits + urdf + sdformat_urdf +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) rcutils rcutils diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 6b58e365dc..34feb57b31 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -14,7 +14,10 @@ #include "hardware_interface/actuator.hpp" +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include @@ -23,9 +26,17 @@ #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +======= +#include "hardware_interface/lifecycle_helpers.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -36,6 +47,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} +<<<<<<< HEAD const rclcpp_lifecycle::State & Actuator::initialize(const HardwareInfo & actuator_info) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) @@ -44,21 +56,53 @@ const rclcpp_lifecycle::State & Actuator::initialize(const HardwareInfo & actuat { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= +Actuator::Actuator(Actuator && other) noexcept +{ + std::lock_guard lock(other.actuators_mutex_); + impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; + last_write_cycle_time_ = other.last_write_cycle_time_; +} + +const rclcpp_lifecycle::State & Actuator::initialize( + const HardwareInfo & actuator_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + std::unique_lock lock(actuators_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->init(actuator_info, logger, clock_interface)) + { + case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_write_cycle_time_ = clock_interface->get_clock()->now(); + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(rclcpp_lifecycle::State( +======= + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } +<<<<<<< HEAD return impl_->get_state(); +======= + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Actuator::configure() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -69,39 +113,79 @@ const rclcpp_lifecycle::State & Actuator::configure() break; case CallbackReturn::FAILURE: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(actuators_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Actuator::cleanup() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(actuators_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Actuator::shutdown() { +<<<<<<< HEAD if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -110,19 +194,39 @@ const rclcpp_lifecycle::State & Actuator::shutdown() { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(actuators_mutex_); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Actuator::activate() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -141,10 +245,32 @@ const rclcpp_lifecycle::State & Actuator::activate() } } return impl_->get_state(); +======= + std::unique_lock lock(actuators_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_activate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Actuator::deactivate() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -163,26 +289,64 @@ const rclcpp_lifecycle::State & Actuator::deactivate() } } return impl_->get_state(); +======= + std::unique_lock lock(actuators_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Actuator::error() { +<<<<<<< HEAD if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(actuators_mutex_); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_error(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(rclcpp_lifecycle::State( +======= + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } +<<<<<<< HEAD return impl_->get_state(); } @@ -196,6 +360,64 @@ std::vector Actuator::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here return impl_->export_command_interfaces(); +======= + return impl_->get_lifecycle_state(); +} + +std::vector Actuator::export_state_interfaces() +{ + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility +} + +std::vector Actuator::export_command_interfaces() +{ + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_command_interfaces(); + // END: for backward compatibility + + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return_type Actuator::prepare_command_mode_switch( @@ -214,6 +436,7 @@ return_type Actuator::perform_command_mode_switch( std::string Actuator::get_name() const { return impl_->get_name(); } +<<<<<<< HEAD const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); } return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -222,42 +445,88 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { +======= +std::string Actuator::get_group_name() const { return impl_->get_group_name(); } + +const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} + +const rclcpp::Time & Actuator::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } + +return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_read_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } return_type result = return_type::ERROR; if ( +<<<<<<< HEAD impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { result = impl_->read(time, period); if (result == return_type::ERROR) { error(); } +<<<<<<< HEAD +======= + last_read_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { +<<<<<<< HEAD if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { +======= + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_write_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } return_type result = return_type::ERROR; if ( +<<<<<<< HEAD impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { result = impl_->write(time, period); if (result == return_type::ERROR) { error(); } +<<<<<<< HEAD +======= + last_write_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } +<<<<<<< HEAD +======= +std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace hardware_interface diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 45ee52c852..ff1d1ab50c 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -13,7 +13,10 @@ // limitations under the License. #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include @@ -21,17 +24,37 @@ #include #include +<<<<<<< HEAD #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lexical_casts.hpp" +======= +#include "urdf/model.h" + +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits_urdf.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace { constexpr const auto kRobotTag = "robot"; +<<<<<<< HEAD constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; constexpr const auto kClassTypeTag = "plugin"; constexpr const auto kParamTag = "param"; +======= +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"; +constexpr const auto kParamTag = "param"; +constexpr const auto kGroupTag = "group"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) constexpr const auto kActuatorTag = "actuator"; constexpr const auto kJointTag = "joint"; constexpr const auto kSensorTag = "sensor"; @@ -41,7 +64,14 @@ constexpr const auto kCommandInterfaceTag = "command_interface"; constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; +<<<<<<< HEAD constexpr const auto kInitialValueTag = "initial_value"; +======= +constexpr const auto kLimitsTag = "limits"; +constexpr const auto kEnableAttribute = "enable"; +constexpr const auto kInitialValueTag = "initial_value"; +constexpr const auto kMimicAttribute = "mimic"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) constexpr const auto kDataTypeAttribute = "data_type"; constexpr const auto kSizeAttribute = "size"; constexpr const auto kNameAttribute = "name"; @@ -49,6 +79,12 @@ constexpr const auto kTypeAttribute = "type"; constexpr const auto kRoleAttribute = "role"; constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; +<<<<<<< HEAD +======= +constexpr const auto kReadWriteRateAttribute = "rw_rate"; +constexpr const auto kIsAsyncAttribute = "is_async"; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace namespace hardware_interface @@ -147,6 +183,10 @@ double get_parameter_value_or( params_it = params_it->NextSiblingElement(); } +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return default_value; } @@ -210,6 +250,59 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) return data_type; } +<<<<<<< HEAD +======= +/// Parse rw_rate attribute +/** + * Parses an XMLElement and returns the value of the rw_rate attribute. + * Defaults to 0 if not specified. + * + * \param[in] elem XMLElement that has the rw_rate attribute. + * \return unsigned int specifying the read/write rate. + */ +unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute); + try + { + const auto rw_rate = attr ? std::stoi(attr->Value()) : 0; + if (rw_rate < 0) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + std::to_string(rw_rate) + "\", but expected a positive integer."); + } + return static_cast(rw_rate); + } + catch (const std::invalid_argument & e) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + + " Invalid value : \"" + attr->Value() + "\", expected a positive integer."); + } + catch (const std::out_of_range & e) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + + " Out of range value : \"" + attr->Value() + "\", expected a positive valid integer."); + } +} + +/// Parse is_async attribute +/** + * Parses an XMLElement and returns the value of the is_async attribute. + * Defaults to "false" if not specified. + * + * \param[in] elem XMLElement that has the data_type attribute. + * \return boolean specifying the if the value read was true or false. + */ +bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kIsAsyncAttribute); + return attr ? parse_bool(attr->Value()) : false; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -269,6 +362,18 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.max = interface_param->second; } +<<<<<<< HEAD +======= + // Option enable or disable the interface limits, by default they are enabled + interface.enable_limits = true; + const auto * limits_it = interfaces_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + interface.enable_limits = + parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Optional initial_value attribute interface_param = interface_params.find(kInitialValueTag); if (interface_param != interface_params.end()) @@ -280,6 +385,16 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.data_type = "double"; interface.size = 1; +<<<<<<< HEAD +======= + // Parse parameters + const auto * params_it = interfaces_it->FirstChildElement(kParamTag); + if (params_it) + { + interface.parameters = parse_parameters_from_xml(params_it); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return interface; } @@ -298,11 +413,43 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it component.type = component_it->Name(); component.name = get_attribute_value(component_it, kNameAttribute, component.type); +<<<<<<< HEAD +======= + if (std::string(kJointTag) == component.type) + { + try + { + component.is_mimic = parse_bool(get_attribute_value(component_it, kMimicAttribute, kJointTag)) + ? MimicAttribute::TRUE + : MimicAttribute::FALSE; + } + catch (const std::runtime_error & e) + { + // mimic attribute not set + component.is_mimic = MimicAttribute::NOT_SET; + } + } + + // Option enable or disable the interface limits, by default they are enabled + bool enable_limits = true; + const auto * limits_it = component_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + enable_limits = parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); while (command_interfaces_it) { +<<<<<<< HEAD component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); +======= + InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it); + cmd_info.enable_limits &= enable_limits; + component.command_interfaces.push_back(cmd_info); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } @@ -310,7 +457,13 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag); while (state_interfaces_it) { +<<<<<<< HEAD component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it)); +======= + InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it); + state_info.enable_limits &= enable_limits; + component.state_interfaces.push_back(state_info); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } @@ -330,7 +483,11 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it * and the interface may be an array of a fixed size of the data type. * * \param[in] component_it pointer to the iterator where component +<<<<<<< HEAD * info should befound +======= + * info should be found +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * \throws std::runtime_error if a required component attribute or tag is not found. */ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * component_it) @@ -410,8 +567,13 @@ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transm // Find name, type and class of a transmission transmission.name = get_attribute_value(transmission_it, kNameAttribute, transmission_it->Name()); +<<<<<<< HEAD const auto * type_it = transmission_it->FirstChildElement(kClassTypeTag); transmission.type = get_text_for_element(type_it, kClassTypeTag); +======= + const auto * type_it = transmission_it->FirstChildElement(kPluginNameTag); + transmission.type = get_text_for_element(type_it, kPluginNameTag); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Parse joints const auto * joint_it = transmission_it->FirstChildElement(kJointTag); @@ -506,6 +668,7 @@ HardwareInfo parse_resource_from_xml( HardwareInfo hardware; hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag); hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); +<<<<<<< HEAD // Parse everything under ros2_control tag hardware.hardware_class_type = ""; @@ -517,12 +680,33 @@ HardwareInfo parse_resource_from_xml( const auto * type_it = ros2_control_child_it->FirstChildElement(kClassTypeTag); hardware.hardware_class_type = get_text_for_element(type_it, std::string("hardware ") + kClassTypeTag); +======= + hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it); + hardware.is_async = parse_is_async_attribute(ros2_control_it); + + // Parse everything under ros2_control tag + hardware.hardware_plugin_name = ""; + const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(); + while (ros2_control_child_it) + { + if (std::string(kHardwareTag) == ros2_control_child_it->Name()) + { + const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag); + hardware.hardware_plugin_name = + get_text_for_element(type_it, std::string("hardware ") + kPluginNameTag); + const auto * group_it = ros2_control_child_it->FirstChildElement(kGroupTag); + if (group_it) + { + hardware.group = get_text_for_element(group_it, std::string("hardware.") + kGroupTag); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); if (params_it) { hardware.hardware_parameters = parse_parameters_from_xml(params_it); } } +<<<<<<< HEAD else if (!std::string(kJointTag).compare(ros2_control_child_it->Name())) { hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it)); @@ -536,6 +720,21 @@ HardwareInfo parse_resource_from_xml( hardware.gpios.push_back(parse_complex_component_from_xml(ros2_control_child_it)); } else if (!std::string(kTransmissionTag).compare(ros2_control_child_it->Name())) +======= + else if (std::string(kJointTag) == ros2_control_child_it->Name()) + { + hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it)); + } + else if (std::string(kSensorTag) == ros2_control_child_it->Name()) + { + hardware.sensors.push_back(parse_component_from_xml(ros2_control_child_it)); + } + else if (std::string(kGPIOTag) == ros2_control_child_it->Name()) + { + hardware.gpios.push_back(parse_complex_component_from_xml(ros2_control_child_it)); + } + else if (std::string(kTransmissionTag) == ros2_control_child_it->Name()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { hardware.transmissions.push_back(parse_transmission_from_xml(ros2_control_child_it)); } @@ -553,6 +752,174 @@ HardwareInfo parse_resource_from_xml( return hardware; } +<<<<<<< HEAD +======= +/** + * @brief Retrieve the min and max values from the interface tag. + * @param itf The interface tag to retrieve the values from. + * @param min The minimum value to be retrieved. + * @param max The maximum value to be retrieved. + * @return true if the values are retrieved, false otherwise. + */ +bool retrieve_min_max_interface_values(const InterfaceInfo & itf, double & min, double & max) +{ + try + { + if (itf.min.empty() && itf.max.empty()) + { + // If the limits don't exist then return false as they are not retrieved + return false; + } + if (!itf.min.empty()) + { + min = hardware_interface::stod(itf.min); + } + if (!itf.max.empty()) + { + max = hardware_interface::stod(itf.max); + } + return true; + } + catch (const std::invalid_argument & err) + { + std::cerr << "Error parsing the limits for the interface: " << itf.name << " from the tags [" + << kMinTag << ": '" << itf.min << "' and " << kMaxTag << ": '" << itf.max + << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" + << std::endl; + return false; + } +} + +/** + * @brief Set custom values for acceleration and jerk limits (no URDF standard) + * @param itr The interface tag to retrieve the values from. + * @param limits The joint limits to be set. + */ +void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointLimits & limits) +{ + if (itr.name == hardware_interface::HW_IF_ACCELERATION) + { + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (detail::retrieve_min_max_interface_values(itr, max_decel, max_accel)) + { + if (std::isfinite(max_decel)) + { + limits.max_deceleration = std::fabs(max_decel); + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + limits.has_deceleration_limits = itr.enable_limits; + } + if (std::isfinite(max_accel)) + { + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = std::fabs(limits.max_acceleration); + } + limits.has_acceleration_limits = itr.enable_limits; + } + } + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + std::cerr << "Error parsing the limits for the interface: " << itr.name + << " from the tag: " << kMinTag << " within " << kROS2ControlTag + << " tag inside the URDF. Jerk only accepts max limits." << std::endl; + } + double min_jerk(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if ( + !itr.max.empty() && detail::retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) + { + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = itr.enable_limits; + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + std::cerr << "Unable to parse the limits for the interface: " << itr.name + << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are: " + "position, velocity, effort, acceleration and jerk." + << std::endl; + } + } +} + +/** + * @brief Retrieve the limits from ros2_control command interface tags and override URDF limits if + * restrictive + * @param interfaces The interfaces to retrieve the limits from. + * @param limits The joint limits to be set. + */ +void update_interface_limits( + const std::vector & interfaces, joint_limits::JointLimits & limits) +{ + for (auto & itr : interfaces) + { + if (itr.name == hardware_interface::HW_IF_POSITION) + { + limits.min_position = limits.has_position_limits && itr.enable_limits + ? limits.min_position + : -std::numeric_limits::max(); + limits.max_position = limits.has_position_limits && itr.enable_limits + ? limits.max_position + : std::numeric_limits::max(); + double min_pos(limits.min_position), max_pos(limits.max_position); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_pos, max_pos)) + { + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + limits.has_position_limits = true; + } + limits.has_position_limits &= itr.enable_limits; + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + limits.max_velocity = + limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) + { + max_vel = std::min(std::abs(min_vel), max_vel); + limits.max_velocity = std::min(max_vel, limits.max_velocity); + limits.has_velocity_limits = true; + } + limits.has_velocity_limits &= itr.enable_limits; + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + limits.max_effort = + limits.has_effort_limits ? limits.max_effort : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_eff(-limits.max_effort), max_eff(limits.max_effort); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + max_eff = std::min(std::abs(min_eff), max_eff); + limits.max_effort = std::min(max_eff, limits.max_effort); + limits.has_effort_limits = true; + } + limits.has_effort_limits &= itr.enable_limits; + } + else + { + detail::set_custom_interface_values(itr, limits); + } + } +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace detail std::vector parse_control_resources_from_urdf(const std::string & urdf) @@ -574,6 +941,7 @@ std::vector parse_control_resources_from_urdf(const std::string & "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); } +<<<<<<< HEAD // Find robot tag const tinyxml2::XMLElement * robot_it = doc.RootElement(); @@ -583,6 +951,28 @@ std::vector parse_control_resources_from_urdf(const std::string & } const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); +======= + // 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()) + { + 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"); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (!ros2_control_it) { throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); @@ -595,7 +985,155 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } +<<<<<<< HEAD + return hardware_info; +} + +======= + // parse full URDF for mimic options + urdf::Model model; + if (!model.initString(urdf)) + { + throw std::runtime_error("Failed to parse URDF file"); + } + for (auto & hw_info : hardware_info) + { + for (auto i = 0u; i < hw_info.joints.size(); ++i) + { + const auto & joint = hw_info.joints.at(i); + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) + { + throw std::runtime_error("Joint '" + joint.name + "' not found in URDF"); + } + if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) + { + throw std::runtime_error( + "Joint '" + joint.name + "' has no mimic information in the URDF."); + } + if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) + { + if (joint.command_interfaces.size() > 0) + { + throw std::runtime_error( + "Joint '" + joint.name + + "' has mimic attribute not set to false: Activated mimic joints cannot have command " + "interfaces."); + } + auto find_joint = [&hw_info](const std::string & name) + { + auto it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&name](const auto & j) { return j.name == name; }); + if (it == hw_info.joints.end()) + { + throw std::runtime_error("Mimic joint '" + name + "' not found in tag"); + } + return static_cast(std::distance(hw_info.joints.begin(), it)); + }; + + MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); + mimic_joint.multiplier = urdf_joint->mimic->multiplier; + mimic_joint.offset = urdf_joint->mimic->offset; + hw_info.mimic_joints.push_back(mimic_joint); + } + + if (urdf_joint->type == urdf::Joint::FIXED) + { + throw std::runtime_error( + "Joint '" + joint.name + + "' is of type 'fixed'. " + "Fixed joints do not make sense in ros2_control."); + } + + joint_limits::JointLimits limits; + getJointLimits(urdf_joint, limits); + // Take the most restricted one. Also valid for continuous-joint type only + detail::update_interface_limits(joint.command_interfaces, limits); + hw_info.limits[joint.name] = limits; + joint_limits::SoftJointLimits soft_limits; + if (getSoftJointLimits(urdf_joint, soft_limits)) + { + if (limits.has_position_limits) + { + soft_limits.min_position = std::max(soft_limits.min_position, limits.min_position); + soft_limits.max_position = std::min(soft_limits.max_position, limits.max_position); + } + hw_info.soft_limits[joint.name] = soft_limits; + } + } + } + return hardware_info; } +std::vector parse_state_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); + } + } + 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) +{ + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); + } + } + 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)); + } + } +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace hardware_interface diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp index c9adcccf83..170d9b6782 100644 --- a/hardware_interface/src/lexical_casts.cpp +++ b/hardware_interface/src/lexical_casts.cpp @@ -12,24 +12,67 @@ // See the License for the specific language governing permissions and // limitations under the License. +<<<<<<< HEAD +======= +#include +#include +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/lexical_casts.hpp" namespace hardware_interface { +<<<<<<< HEAD double stod(const std::string & s) { // convert from string using no locale +======= +namespace impl +{ +std::optional stod(const std::string & s) +{ +#if __cplusplus < 202002L + // convert from string using no locale + // Impl with std::istringstream +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::istringstream stream(s); stream.imbue(std::locale::classic()); double result; stream >> result; if (stream.fail() || !stream.eof()) { +<<<<<<< HEAD throw std::invalid_argument("Failed converting string to real number"); } return result; } +======= + return std::nullopt; + } + return result; +#else + // Impl with std::from_chars + double result_value; + const auto parse_result = std::from_chars(s.data(), s.data() + s.size(), result_value); + if (parse_result.ec == std::errc()) + { + return result_value; + } + return std::nullopt; +#endif +} +} // namespace impl +double stod(const std::string & s) +{ + if (const auto result = impl::stod(s)) + { + return *result; + } + throw std::invalid_argument("Failed converting string to real number"); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) bool parse_bool(const std::string & bool_string) { return bool_string == "true" || bool_string == "True"; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 3fb9999b24..881a6385f1 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -20,6 +20,7 @@ #include #include #include +<<<<<<< HEAD #include #include #include @@ -31,6 +32,18 @@ namespace mock_components { +======= +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" + +namespace mock_components +{ + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) @@ -60,13 +73,19 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i }; // check if to create mock command interface for sensor +<<<<<<< HEAD auto it = info_.hardware_parameters.find("mock_sensor_commands"); if (it != info_.hardware_parameters.end()) +======= + auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands"); + if (it != get_hardware_info().hardware_parameters.end()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { +<<<<<<< HEAD // check if fake_sensor_commands was set instead and issue warning. it = info_.hardware_parameters.find("fake_sensor_commands"); if (it != info_.hardware_parameters.end()) @@ -86,11 +105,20 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // check if to create mock command interface for gpio it = info_.hardware_parameters.find("mock_gpio_commands"); if (it != info_.hardware_parameters.end()) +======= + use_mock_sensor_command_interfaces_ = false; + } + + // check if to create mock command interface for gpio + it = get_hardware_info().hardware_parameters.find("mock_gpio_commands"); + if (it != get_hardware_info().hardware_parameters.end()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { +<<<<<<< HEAD // check if fake_gpio_commands was set instead and issue warning it = info_.hardware_parameters.find("fake_gpio_commands"); if (it != info_.hardware_parameters.end()) @@ -105,11 +133,18 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i { use_mock_gpio_command_interfaces_ = false; } +======= + use_mock_gpio_command_interfaces_ = false; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // check if there is parameter that disables commands // this way we simulate disconnected driver +<<<<<<< HEAD it = info_.hardware_parameters.find("disable_commands"); +======= + it = get_hardware_info().hardware_parameters.find("disable_commands"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (it != info.hardware_parameters.end()) { command_propagation_disabled_ = hardware_interface::parse_bool(it->second); @@ -120,7 +155,11 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if there is parameter that enables dynamic calculation +<<<<<<< HEAD it = info_.hardware_parameters.find("calculate_dynamics"); +======= + it = get_hardware_info().hardware_parameters.find("calculate_dynamics"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (it != info.hardware_parameters.end()) { calculate_dynamics_ = hardware_interface::parse_bool(it->second); @@ -134,12 +173,21 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; +<<<<<<< HEAD it = info_.hardware_parameters.find("position_state_following_offset"); if (it != info_.hardware_parameters.end()) { position_state_following_offset_ = hardware_interface::stod(it->second); it = info_.hardware_parameters.find("custom_interface_with_following_offset"); if (it != info_.hardware_parameters.end()) +======= + it = get_hardware_info().hardware_parameters.find("position_state_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) + { + position_state_following_offset_ = hardware_interface::stod(it->second); + it = get_hardware_info().hardware_parameters.find("custom_interface_with_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { custom_interface_with_following_offset_ = it->second; } @@ -148,9 +196,16 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces +<<<<<<< HEAD initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); // set all values without initial values to 0 for (auto i = 0u; i < info_.joints.size(); i++) +======= + initialize_storage_vectors( + joint_commands_, joint_states_, standard_interfaces_, get_hardware_info().joints); + // set all values without initial values to 0 + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { for (auto j = 0u; j < standard_interfaces_.size(); j++) { @@ -161,6 +216,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } +<<<<<<< HEAD // Search for mimic joints for (auto i = 0u; i < info_.joints.size(); ++i) { @@ -192,6 +248,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // search for non-standard joint interfaces for (const auto & joint : info_.joints) +======= + // search for non-standard joint interfaces + for (const auto & joint : get_hardware_info().joints) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // populate non-standard command interfaces to other_interfaces_ populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); @@ -201,7 +261,12 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // Initialize storage for non-standard interfaces +<<<<<<< HEAD initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); +======= + initialize_storage_vectors( + other_commands_, other_states_, other_interfaces_, get_hardware_info().joints); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // when following offset is used on custom interface then find its index if (!custom_interface_with_following_offset_.empty()) @@ -212,21 +277,35 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i { index_custom_interface_with_following_offset_ = static_cast(std::distance(other_interfaces_.begin(), if_it)); +<<<<<<< HEAD RCUTILS_LOG_INFO_NAMED( "mock_generic_system", "Custom interface with following offset '%s' found at index: %zu.", +======= + RCLCPP_INFO( + get_logger(), "Custom interface with following offset '%s' found at index: %zu.", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) custom_interface_with_following_offset_.c_str(), index_custom_interface_with_following_offset_); } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "mock_generic_system", +======= + RCLCPP_WARN( + get_logger(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "Custom interface with following offset '%s' does not exist. Offset will not be applied", custom_interface_with_following_offset_.c_str()); } } +<<<<<<< HEAD for (const auto & sensor : info_.sensors) +======= + for (const auto & sensor : get_hardware_info().sensors) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { for (const auto & interface : sensor.state_interfaces) { @@ -239,10 +318,17 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } initialize_storage_vectors( +<<<<<<< HEAD sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); // search for gpio interfaces for (const auto & gpio : info_.gpios) +======= + sensor_mock_commands_, sensor_states_, sensor_interfaces_, get_hardware_info().sensors); + + // search for gpio interfaces + for (const auto & gpio : get_hardware_info().gpios) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // populate non-standard command interfaces to gpio_interfaces_ populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); @@ -254,12 +340,22 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // Mock gpio command interfaces if (use_mock_gpio_command_interfaces_) { +<<<<<<< HEAD initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); +======= + initialize_storage_vectors( + gpio_mock_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // Real gpio command interfaces else { +<<<<<<< HEAD initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); +======= + initialize_storage_vectors( + gpio_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return CallbackReturn::SUCCESS; @@ -270,9 +366,15 @@ std::vector GenericSystem::export_state_inte std::vector state_interfaces; // Joints' state interfaces +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); i++) { const auto & joint = info_.joints[i]; +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) + { + const auto & joint = get_hardware_info().joints[i]; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & interface : joint.state_interfaces) { // Add interface: if not in the standard list then use "other" interface list @@ -292,14 +394,23 @@ std::vector GenericSystem::export_state_inte // Sensor state interfaces if (!populate_interfaces( +<<<<<<< HEAD info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) +======= + get_hardware_info().sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); }; // GPIO state interfaces +<<<<<<< HEAD if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) +======= + if (!populate_interfaces( + get_hardware_info().gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); } @@ -312,9 +423,15 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces +<<<<<<< HEAD for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; +======= + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + const auto & joint = get_hardware_info().joints[i]; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & interface : joint.command_interfaces) { // Add interface: if not in the standard list than use "other" interface list @@ -334,13 +451,22 @@ std::vector GenericSystem::export_command_ } } // Set position control mode per default +<<<<<<< HEAD joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); +======= + joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { if (!populate_interfaces( +<<<<<<< HEAD info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) +======= + get_hardware_info().sensors, sensor_interfaces_, sensor_mock_commands_, + command_interfaces, true)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); @@ -351,7 +477,12 @@ std::vector GenericSystem::export_command_ if (use_mock_gpio_command_interfaces_) { if (!populate_interfaces( +<<<<<<< HEAD info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) +======= + get_hardware_info().gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, + true)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -361,7 +492,11 @@ std::vector GenericSystem::export_command_ else { if (!populate_interfaces( +<<<<<<< HEAD info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) +======= + get_hardware_info().gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -384,13 +519,20 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; +<<<<<<< HEAD std::vector joint_found_in_x_requests_; joint_found_in_x_requests_.resize(info_.joints.size(), 0); +======= + const auto & info = get_hardware_info(); + std::vector joint_found_in_x_requests_; + joint_found_in_x_requests_.resize(info.joints.size(), 0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (const auto & key : start_interfaces) { // check if interface is joint auto joint_it_found = std::find_if( +<<<<<<< HEAD info_.joints.begin(), info_.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); @@ -398,11 +540,21 @@ return_type GenericSystem::prepare_command_mode_switch( { const size_t joint_index = static_cast(std::distance(info_.joints.begin(), joint_it_found)); +======= + info.joints.begin(), info.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info.joints.end()) + { + const size_t joint_index = + static_cast(std::distance(info.joints.begin(), joint_it_found)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (joint_found_in_x_requests_[joint_index] == 0) { joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; } +<<<<<<< HEAD if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_found_in_x_requests_[joint_index] += 1; @@ -428,12 +580,40 @@ return_type GenericSystem::prepare_command_mode_switch( "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " "this might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); +======= + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + if (!calculate_dynamics_) + { + RCLCPP_WARN( + get_logger(), + "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " + "might lead to wrong feedback and unexpected behavior.", + info.joints[joint_index].name.c_str()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + if (!calculate_dynamics_) + { + RCLCPP_WARN( + get_logger(), + "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " + "this might lead to wrong feedback and unexpected behavior.", + info.joints[joint_index].name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } joint_found_in_x_requests_[joint_index] += 1; } } else { +<<<<<<< HEAD RCUTILS_LOG_DEBUG_NAMED( "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", key.c_str()); @@ -441,13 +621,27 @@ return_type GenericSystem::prepare_command_mode_switch( } for (size_t i = 0; i < info_.joints.size(); ++i) +======= + RCLCPP_DEBUG( + get_logger(), "Got interface '%s' that is not joint - nothing to do!", key.c_str()); + } + } + + for (size_t i = 0; i < info.joints.size(); ++i) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // There has to always be at least one control mode from the above three set if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) { +<<<<<<< HEAD RCUTILS_LOG_ERROR_NAMED( "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, +======= + RCLCPP_ERROR( + get_logger(), "Joint '%s' has to have '%s', '%s', or '%s' interface!", + info.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); ret_val = hardware_interface::return_type::ERROR; } @@ -455,11 +649,19 @@ return_type GenericSystem::prepare_command_mode_switch( // Currently we don't support multiple interface request if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) { +<<<<<<< HEAD RCUTILS_LOG_ERROR_NAMED( "mock_generic_system", "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); +======= + RCLCPP_ERROR( + get_logger(), + "Got multiple (%zu) starting interfaces for joint '%s' - this is not " + "supported!", + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info.joints[i].name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ret_val = hardware_interface::return_type::ERROR; } } @@ -479,6 +681,7 @@ return_type GenericSystem::perform_command_mode_switch( for (const auto & key : start_interfaces) { // check if interface is joint +<<<<<<< HEAD auto joint_it_found = std::find_if( info_.joints.begin(), info_.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); @@ -497,6 +700,27 @@ return_type GenericSystem::perform_command_mode_switch( joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) +======= + const auto & info = get_hardware_info(); + auto joint_it_found = std::find_if( + info.joints.begin(), info.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info.joints.end()) + { + const size_t joint_index = + static_cast(std::distance(info.joints.begin(), joint_it_found)); + + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + } + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + } + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; } @@ -510,12 +734,21 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { if (command_propagation_disabled_) { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "mock_generic_system", "Command propagation is disabled - no values will be returned!"); return return_type::OK; } auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) +======= + RCLCPP_WARN(get_logger(), "Command propagation is disabled - no values will be returned!"); + return return_type::OK; + } + + auto mirror_command_to_state = + [](auto & states_, auto commands_, size_t start_index = 0) -> return_type +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { for (size_t i = start_index; i < states_.size(); ++i) { @@ -525,8 +758,18 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { states_[i][j] = commands_[i][j]; } +<<<<<<< HEAD + } + } +======= + if (std::isinf(commands_[i][j])) + { + return return_type::ERROR; + } } } + return return_type::OK; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) @@ -612,6 +855,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, // velocity, and acceleration interface +<<<<<<< HEAD if (calculate_dynamics_) { mirror_command_to_state(joint_states_, joint_commands_, 3); @@ -622,10 +866,24 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } for (const auto & mimic_joint : mimic_joints_) +======= + if ( + mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) != + return_type::OK) + { + return return_type::ERROR; + } + + for (const auto & mimic_joint : get_hardware_info().mimic_joints) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { for (auto i = 0u; i < joint_states_.size(); ++i) { joint_states_[i][mimic_joint.joint_index] = +<<<<<<< HEAD +======= + mimic_joint.offset + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; } } @@ -653,13 +911,20 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mirror_command_to_state(sensor_states_, sensor_mock_commands_); } +<<<<<<< HEAD // do loopback on all gpio interfaces +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (use_mock_gpio_command_interfaces_) { mirror_command_to_state(gpio_states_, gpio_mock_commands_); } else { +<<<<<<< HEAD +======= + // do loopback on all gpio interfaces +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mirror_command_to_state(gpio_states_, gpio_commands_); } @@ -698,7 +963,10 @@ void GenericSystem::initialize_storage_vectors( } // Initialize with values from URDF +<<<<<<< HEAD bool print_hint = false; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) for (auto i = 0u; i < component_infos.size(); i++) { const auto & component = component_infos[i]; @@ -716,6 +984,7 @@ void GenericSystem::initialize_storage_vectors( { states[index][i] = hardware_interface::stod(interface.initial_value); } +<<<<<<< HEAD else { // Initialize the value in old way with warning message @@ -743,6 +1012,11 @@ void GenericSystem::initialize_storage_vectors( " 0.0 \n" ""); } +======= + } + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } template diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 06c56b4cce..8980b58117 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -17,22 +17,38 @@ #include #include #include +<<<<<<< HEAD #include +======= +#include +#include +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include #include #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" +<<<<<<< HEAD +======= +#include "hardware_interface/async_components.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +======= +#include "lifecycle_msgs/msg/state.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rcutils/logging_macros.h" namespace hardware_interface @@ -57,13 +73,40 @@ auto trigger_and_print_hardware_state_transition = } else { +<<<<<<< HEAD RCUTILS_LOG_INFO_NAMED( +======= + RCUTILS_LOG_ERROR_NAMED( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(), hardware_name.c_str()); } return result; }; +<<<<<<< HEAD +======= +std::string interfaces_to_string( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + std::stringstream ss; + ss << "Start interfaces: " << std::endl << "[" << std::endl; + for (const auto & start_if : start_interfaces) + { + ss << " " << start_if << std::endl; + } + ss << "]" << std::endl; + ss << "Stop interfaces: " << std::endl << "[" << std::endl; + for (const auto & stop_if : stop_interfaces) + { + ss << " " << stop_if << std::endl; + } + ss << "]" << std::endl; + return ss.str(); +}; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class ResourceStorage { static constexpr const char * pkg_name = "hardware_interface"; @@ -73,6 +116,7 @@ class ResourceStorage static constexpr const char * system_interface_name = "hardware_interface::SystemInterface"; public: +<<<<<<< HEAD ResourceStorage() : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), @@ -103,11 +147,99 @@ class ResourceStorage hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hardware_used_by_controllers_.insert( std::make_pair(component_info.name, std::vector())); +======= + // TODO(VX792): Change this when HW ifs get their own update rate, + // because the ResourceStorage really shouldn't know about the cm's parameters + explicit ResourceStorage( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + : actuator_loader_(pkg_name, actuator_interface_name), + sensor_loader_(pkg_name, sensor_interface_name), + system_loader_(pkg_name, system_interface_name), + clock_interface_(clock_interface), + rm_logger_(rclcpp::get_logger("resource_manager")) + { + if (!clock_interface_) + { + throw std::invalid_argument( + "Clock interface is nullptr. ResourceManager needs a valid clock interface."); + } + if (logger_interface) + { + rm_logger_ = logger_interface->get_logger().get_child("resource_manager"); + } + } + + template + [[nodiscard]] bool load_hardware( + const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, + std::vector & container) + { + bool is_loaded = false; + try + { + RCLCPP_INFO(get_logger(), "Loading hardware '%s' ", hardware_info.name.c_str()); + // hardware_plugin_name has to match class name in plugin xml description + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); + if (interface) + { + RCLCPP_INFO( + get_logger(), "Loaded hardware '%s' from plugin '%s'", hardware_info.name.c_str(), + hardware_info.hardware_plugin_name.c_str()); + HardwareT hardware(std::move(interface)); + container.emplace_back(std::move(hardware)); + // initialize static data about hardware component to reduce later calls + HardwareComponentInfo component_info; + component_info.name = hardware_info.name; + component_info.type = hardware_info.type; + component_info.group = hardware_info.group; + component_info.rw_rate = + (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_) + ? cm_update_rate_ + : hardware_info.rw_rate; + component_info.plugin_name = hardware_info.hardware_plugin_name; + component_info.is_async = hardware_info.is_async; + + hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); + hw_group_state_.insert(std::make_pair(component_info.group, return_type::OK)); + hardware_used_by_controllers_.insert( + std::make_pair(component_info.name, std::vector())); + is_loaded = true; + } + else + { + RCLCPP_ERROR( + get_logger(), "Failed to load hardware '%s' from plugin '%s'", hardware_info.name.c_str(), + hardware_info.hardware_plugin_name.c_str()); + } + } + catch (const pluginlib::PluginlibException & ex) + { + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while loading hardware: %s", typeid(ex).name(), + ex.what()); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while loading hardware '%s': %s", + typeid(ex).name(), hardware_info.name.c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while loading hardware '%s'", + hardware_info.name.c_str()); + } + return is_loaded; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } template bool initialize_hardware(const HardwareInfo & hardware_info, HardwareT & hardware) { +<<<<<<< HEAD RCUTILS_LOG_INFO_NAMED( "resource_manager", "Initialize hardware '%s' ", hardware_info.name.c_str()); @@ -126,15 +258,72 @@ class ResourceStorage RCUTILS_LOG_INFO_NAMED( "resource_manager", "Failed to initialize hardware '%s'", hardware_info.name.c_str()); } +======= + RCLCPP_INFO(get_logger(), "Initialize hardware '%s' ", hardware_info.name.c_str()); + + bool result = false; + try + { + const rclcpp_lifecycle::State new_state = + hardware.initialize(hardware_info, rm_logger_, clock_interface_); + result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; + + if (result) + { + RCLCPP_INFO( + get_logger(), "Successful initialization of hardware '%s'", hardware_info.name.c_str()); + } + else + { + RCLCPP_ERROR( + get_logger(), "Failed to initialize hardware '%s'", hardware_info.name.c_str()); + } + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while initializing hardware '%s': %s", + typeid(ex).name(), hardware_info.name.c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while initializing hardware '%s'", + hardware_info.name.c_str()); + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return result; } template bool configure_hardware(HardwareT & hardware) { +<<<<<<< HEAD bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::configure, &hardware), "configure", hardware.get_name(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +======= + bool result = false; + try + { + result = trigger_and_print_hardware_state_transition( + std::bind(&HardwareT::configure, &hardware), "configure", hardware.get_name(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while configuring hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while configuring hardware '%s'", + hardware.get_name().c_str()); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (result) { @@ -152,16 +341,26 @@ class ResourceStorage if (found_it == available_state_interfaces_.end()) { available_state_interfaces_.emplace_back(interface); +<<<<<<< HEAD RCUTILS_LOG_DEBUG_NAMED( "resource_manager", "(hardware '%s'): '%s' state interface added into available list", +======= + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' state interface added into available list", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware.get_name().c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", +======= + RCLCPP_WARN( + get_logger(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "(hardware '%s'): '%s' state interface already in available list." " This can happen due to multiple calls to 'configure'", hardware.get_name().c_str(), interface.c_str()); @@ -178,21 +377,47 @@ class ResourceStorage if (found_it == available_command_interfaces_.end()) { available_command_interfaces_.emplace_back(interface); +<<<<<<< HEAD RCUTILS_LOG_DEBUG_NAMED( "resource_manager", "(hardware '%s'): '%s' command interface added into available list", +======= + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' command interface added into available list", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware.get_name().c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", +======= + RCLCPP_WARN( + get_logger(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "(hardware '%s'): '%s' command interface already in available list." " This can happen due to multiple calls to 'configure'", hardware.get_name().c_str(), interface.c_str()); } } +<<<<<<< HEAD +======= + + 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()) + { + hw_group_state_[hardware.get_group_name()] = return_type::OK; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } @@ -208,16 +433,26 @@ class ResourceStorage if (found_it != available_command_interfaces_.end()) { available_command_interfaces_.erase(found_it); +<<<<<<< HEAD RCUTILS_LOG_DEBUG_NAMED( "resource_manager", "(hardware '%s'): '%s' command interface removed from available list", +======= + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' command interface removed from available list", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_name.c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", +======= + RCLCPP_WARN( + get_logger(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "(hardware '%s'): '%s' command interface not in available list. " "This should not happen (hint: multiple cleanup calls).", hardware_name.c_str(), interface.c_str()); @@ -232,16 +467,26 @@ class ResourceStorage if (found_it != available_state_interfaces_.end()) { available_state_interfaces_.erase(found_it); +<<<<<<< HEAD RCUTILS_LOG_DEBUG_NAMED( "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", +======= + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' state interface removed from available list", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_name.c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", +======= + RCLCPP_WARN( + get_logger(), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "(hardware '%s'): '%s' state interface not in available list. " "This should not happen (hint: multiple cleanup calls).", hardware_name.c_str(), interface.c_str()); @@ -252,30 +497,93 @@ class ResourceStorage template bool cleanup_hardware(HardwareT & hardware) { +<<<<<<< HEAD bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::cleanup, &hardware), "cleanup", hardware.get_name(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +======= + bool result = false; + try + { + result = trigger_and_print_hardware_state_transition( + std::bind(&HardwareT::cleanup, &hardware), "cleanup", hardware.get_name(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while cleaning up hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while cleaning up hardware '%s'", + hardware.get_name().c_str()); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (result) { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); } +<<<<<<< HEAD +======= + if (!hardware.get_group_name().empty()) + { + hw_group_state_[hardware.get_group_name()] = return_type::OK; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return result; } template bool shutdown_hardware(HardwareT & hardware) { +<<<<<<< HEAD bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::shutdown, &hardware), "shutdown", hardware.get_name(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); if (result) { +======= + bool result = false; + try + { + result = trigger_and_print_hardware_state_transition( + std::bind(&HardwareT::shutdown, &hardware), "shutdown", hardware.get_name(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while shutting down hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while shutting down hardware '%s'", + hardware.get_name().c_str()); + } + + if (result) + { + remove_all_hardware_interfaces_from_available_list(hardware.get_name()); + async_component_threads_.erase(hardware.get_name()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); // use remove_command_interfaces(hardware); +<<<<<<< HEAD +======= + if (!hardware.get_group_name().empty()) + { + hw_group_state_[hardware.get_group_name()] = return_type::OK; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } @@ -283,12 +591,41 @@ class ResourceStorage template bool activate_hardware(HardwareT & hardware) { +<<<<<<< HEAD bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::activate, &hardware), "activate", hardware.get_name(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); if (result) { +======= + bool result = false; + try + { + result = trigger_and_print_hardware_state_transition( + std::bind(&HardwareT::activate, &hardware), "activate", hardware.get_name(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while activating hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + 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(); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // TODO(destogl): make all command interfaces available (currently are all available) } @@ -298,9 +635,31 @@ class ResourceStorage template bool deactivate_hardware(HardwareT & hardware) { +<<<<<<< HEAD bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::deactivate, &hardware), "deactivate", hardware.get_name(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +======= + bool result = false; + try + { + result = trigger_and_print_hardware_state_transition( + std::bind(&HardwareT::deactivate, &hardware), "deactivate", hardware.get_name(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s occurred while deactivating hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while deactivating hardware '%s'", + hardware.get_name().c_str()); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (result) { @@ -320,7 +679,11 @@ class ResourceStorage switch (target_state.id()) { case State::PRIMARY_STATE_UNCONFIGURED: +<<<<<<< HEAD switch (hardware.get_state().id()) +======= + switch (hardware.get_lifecycle_state().id()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { case State::PRIMARY_STATE_UNCONFIGURED: result = true; @@ -337,14 +700,23 @@ class ResourceStorage break; case State::PRIMARY_STATE_FINALIZED: result = false; +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "hardware '%s' is in finalized state and can be only destroyed.", +======= + RCLCPP_WARN( + get_logger(), "hardware '%s' is in finalized state and can be only destroyed.", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware.get_name().c_str()); break; } break; case State::PRIMARY_STATE_INACTIVE: +<<<<<<< HEAD switch (hardware.get_state().id()) +======= + switch (hardware.get_lifecycle_state().id()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -357,14 +729,23 @@ class ResourceStorage break; case State::PRIMARY_STATE_FINALIZED: result = false; +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "hardware '%s' is in finalized state and can be only destroyed.", +======= + RCLCPP_WARN( + get_logger(), "hardware '%s' is in finalized state and can be only destroyed.", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware.get_name().c_str()); break; } break; case State::PRIMARY_STATE_ACTIVE: +<<<<<<< HEAD switch (hardware.get_state().id()) +======= + switch (hardware.get_lifecycle_state().id()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -381,14 +762,23 @@ class ResourceStorage break; case State::PRIMARY_STATE_FINALIZED: result = false; +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "hardware '%s' is in finalized state and can be only destroyed.", +======= + RCLCPP_WARN( + get_logger(), "hardware '%s' is in finalized state and can be only destroyed.", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware.get_name().c_str()); break; } break; case State::PRIMARY_STATE_FINALIZED: +<<<<<<< HEAD switch (hardware.get_state().id()) +======= + switch (hardware.get_lifecycle_state().id()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { case State::PRIMARY_STATE_UNCONFIGURED: result = shutdown_hardware(hardware); @@ -413,6 +803,7 @@ class ResourceStorage void import_state_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_state_interfaces(); +<<<<<<< HEAD std::vector interface_names; interface_names.reserve(interfaces.size()); for (auto & interface : interfaces) @@ -421,16 +812,144 @@ class ResourceStorage state_interface_map_.emplace(std::make_pair(key, std::move(interface))); interface_names.push_back(key); } +======= + const auto interface_names = add_state_interfaces(interfaces); + + RCLCPP_WARN_EXPRESSION( + get_logger(), interface_names.empty(), + "Importing state interfaces for the hardware '%s' returned no state interfaces.", + hardware.get_name().c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); } +<<<<<<< HEAD template void import_command_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); +======= + void insert_command_interface(const CommandInterface::SharedPtr command_interface) + { + const auto [it, success] = command_interface_map_.insert( + std::make_pair(command_interface->get_name(), command_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + command_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + void insert_command_interface(CommandInterface && command_interface) + { + std::string key = command_interface.get_name(); + const auto [it, success] = command_interface_map_.emplace( + std::make_pair(key, std::make_shared(std::move(command_interface)))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + key + "]"); + throw std::runtime_error(msg); + } + } + // END: for backward compatibility + + template + void import_command_interfaces(HardwareT & hardware) + { + try + { + auto interfaces = hardware.export_command_interfaces(); + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interfaces(interfaces); + // TODO(Manuel) END: for backward compatibility + } + catch (const std::exception & ex) + { + RCLCPP_ERROR( + get_logger(), + "Exception of type : %s occurred while importing command interfaces for the hardware '%s' " + ": %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), + "Unknown exception occurred while importing command interfaces for the hardware '%s'", + hardware.get_name().c_str()); + } + } + + std::string add_state_interface(StateInterface::ConstSharedPtr interface) + { + auto interface_name = interface->get_name(); + const auto [it, success] = state_interface_map_.emplace(interface_name, interface); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + interface->get_name() + "]"); + throw std::runtime_error(msg); + } + return interface_name; + } + /// Adds exported state interfaces into internal storage. + /** + * Adds state interfaces to the internal storage. State interfaces exported from hardware or + * chainable controllers are moved to the map with name-interface pairs and available list's + * size is increased to reserve storage when interface change theirs status in real-time + * control loop. + * + * \param[interfaces] list of state interface to add into storage. + * \returns list of interface names that are added into internal storage. The output is used to + * avoid additional iterations to cache interface names, e.g., for initializing info structures. + */ + std::vector add_state_interfaces( + std::vector & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + try + { + interface_names.push_back(add_state_interface(interface)); + } + // We don't want to crash during runtime because a StateInterface could not be added + catch (const std::exception & e) + { + RCLCPP_WARN( + get_logger(), "Exception occurred while importing state interfaces: %s", e.what()); + } + } + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + + /// Removes state interfaces from internal storage. + /** + * State interface are removed from the maps with theirs storage and their claimed status. + * + * \param[interface_names] list of state interface names to remove from storage. + */ + void remove_state_interfaces(const std::vector & interface_names) + { + for (const auto & interface : interface_names) + { + state_interface_map_.erase(interface); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } /// Adds exported command interfaces into internal storage. @@ -451,7 +970,29 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); +<<<<<<< HEAD command_interface_map_.emplace(std::make_pair(key, std::move(interface))); +======= + insert_command_interface(std::move(interface)); + claimed_command_interface_map_.emplace(std::make_pair(key, false)); + interface_names.push_back(key); + } + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + + std::vector add_command_interfaces( + const std::vector & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (const auto & interface : interfaces) + { + auto key = interface->get_name(); + insert_command_interface(interface); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -476,6 +1017,7 @@ class ResourceStorage } } +<<<<<<< HEAD void check_for_duplicates(const HardwareInfo & hardware_info) { // Check for identical names @@ -494,6 +1036,17 @@ class ResourceStorage { check_for_duplicates(hardware_info); load_hardware(hardware_info, actuator_loader_, container); +======= + // TODO(destogl): Propagate "false" up, if happens in initialize_hardware + bool load_and_initialize_actuator(const HardwareInfo & hardware_info) + { + auto load_and_init_actuators = [&](auto & container) + { + if (!load_hardware(hardware_info, actuator_loader_, container)) + { + return false; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -501,6 +1054,7 @@ class ResourceStorage } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "Actuator hardware component '%s' from plugin '%s' failed to initialize.", @@ -517,12 +1071,41 @@ class ResourceStorage { check_for_duplicates(hardware_info); load_hardware(hardware_info, sensor_loader_, container); +======= + RCLCPP_WARN( + get_logger(), "Actuator hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; + } + return true; + }; + + if (hardware_info.is_async) + { + return load_and_init_actuators(async_actuators_); + } + else + { + return load_and_init_actuators(actuators_); + } + } + + bool load_and_initialize_sensor(const HardwareInfo & hardware_info) + { + auto load_and_init_sensors = [&](auto & container) + { + if (!load_hardware(hardware_info, sensor_loader_, container)) + { + return false; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", @@ -539,6 +1122,34 @@ class ResourceStorage { check_for_duplicates(hardware_info); load_hardware(hardware_info, system_loader_, container); +======= + RCLCPP_WARN( + get_logger(), "Sensor hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; + } + return true; + }; + + if (hardware_info.is_async) + { + return load_and_init_sensors(async_sensors_); + } + else + { + return load_and_init_sensors(sensors_); + } + } + + bool load_and_initialize_system(const HardwareInfo & hardware_info) + { + auto load_and_init_systems = [&](auto & container) + { + if (!load_hardware(hardware_info, system_loader_, container)) + { + return false; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -546,6 +1157,7 @@ class ResourceStorage } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "System hardware component '%s' from plugin '%s' failed to initialize.", @@ -554,6 +1166,24 @@ class ResourceStorage }; load_and_init_systems(systems_); +======= + RCLCPP_WARN( + get_logger(), "System hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; + } + return true; + }; + + if (hardware_info.is_async) + { + return load_and_init_systems(async_systems_); + } + else + { + return load_and_init_systems(systems_); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void initialize_actuator( @@ -569,6 +1199,7 @@ class ResourceStorage } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "Actuator hardware component '%s' from plugin '%s' failed to initialize.", @@ -577,6 +1208,22 @@ class ResourceStorage }; init_actuators(actuators_); +======= + RCLCPP_WARN( + get_logger(), "Actuator hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + } + }; + + if (hardware_info.is_async) + { + init_actuators(async_actuators_); + } + else + { + init_actuators(actuators_); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void initialize_sensor( @@ -591,6 +1238,7 @@ class ResourceStorage } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", @@ -599,6 +1247,22 @@ class ResourceStorage }; init_sensors(sensors_); +======= + RCLCPP_WARN( + get_logger(), "Sensor hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + } + }; + + if (hardware_info.is_async) + { + init_sensors(async_sensors_); + } + else + { + init_sensors(sensors_); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } void initialize_system( @@ -614,6 +1278,7 @@ class ResourceStorage } else { +<<<<<<< HEAD RCUTILS_LOG_WARN_NAMED( "resource_manager", "System hardware component '%s' from plugin '%s' failed to initialize.", @@ -624,20 +1289,109 @@ class ResourceStorage init_systems(systems_); } +======= + RCLCPP_WARN( + get_logger(), "System hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + } + }; + + if (hardware_info.is_async) + { + init_systems(async_systems_); + } + else + { + init_systems(systems_); + } + } + + void clear() + { + actuators_.clear(); + 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(); + + available_state_interfaces_.clear(); + available_command_interfaces_.clear(); + + claimed_command_interface_map_.clear(); + } + + /** + * Returns the return type of the hardware component group state, if the return type is other + * than OK, then updates the return type of the group to the respective one + */ + return_type update_hardware_component_group_state( + const std::string & group_name, const return_type & value) + { + // This is for the components that has no configured group + if (group_name.empty()) + { + return value; + } + // If it is anything other than OK, change the return type of the hardware group state + // to the respective return type + if (value != return_type::OK) + { + hw_group_state_.at(group_name) = value; + } + return hw_group_state_.at(group_name); + } + + /// Gets the logger for the resource storage + /** + * \return logger of the resource storage + */ + const rclcpp::Logger & get_logger() const { return rm_logger_; } + + /// Gets the clock for the resource storage + /** + * \return clock of the resource storage + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; pluginlib::ClassLoader system_loader_; +<<<<<<< HEAD +======= + // Logger and Clock interfaces + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + rclcpp::Logger rm_logger_; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::vector actuators_; std::vector sensors_; std::vector systems_; +<<<<<<< HEAD + std::unordered_map hardware_info_map_; +======= + std::vector async_actuators_; + std::vector async_sensors_; + std::vector async_systems_; + std::unordered_map hardware_info_map_; + std::unordered_map hw_group_state_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Mapping between hardware and controllers that are using it (accessing data from it) std::unordered_map> hardware_used_by_controllers_; +<<<<<<< HEAD /// Mapping between controllers and list of reference interfaces they are using std::unordered_map> controllers_reference_interfaces_map_; @@ -645,6 +1399,17 @@ class ResourceStorage std::map state_interface_map_; /// Storage of all available command interfaces std::map command_interface_map_; +======= + /// Mapping between controllers and list of interfaces they are using + std::unordered_map> + controllers_exported_state_interfaces_map_; + std::unordered_map> controllers_reference_interfaces_map_; + + /// Storage of all available state interfaces + std::map state_interface_map_; + /// Storage of all available command interfaces + std::map command_interface_map_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -652,17 +1417,44 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; +<<<<<<< HEAD }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} +======= + + /// 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; +}; + +ResourceManager::ResourceManager( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) +: resource_storage_(std::make_unique(clock_interface, logger_interface)) +{ +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( +<<<<<<< HEAD const std::string & urdf, bool validate_interfaces, bool activate_all) : resource_storage_(std::make_unique()) { load_urdf(urdf, validate_interfaces); +======= + const std::string & urdf, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, bool activate_all, + const unsigned int update_rate) +: resource_storage_(std::make_unique(clock_interface, logger_interface)) +{ + load_and_initialize_components(urdf, update_rate); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (activate_all) { @@ -676,14 +1468,27 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread +<<<<<<< HEAD void ResourceManager::load_urdf( const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) { is_urdf_loaded__ = true; +======= +bool ResourceManager::load_and_initialize_components( + const std::string & urdf, const unsigned int update_rate) +{ + components_are_loaded_and_initialized_ = true; + + resource_storage_->cm_update_rate_ = update_rate; + + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; +<<<<<<< HEAD const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); if (load_and_initialize_components) std::lock_guard resource_guard(resources_lock_); @@ -706,10 +1511,55 @@ void ResourceManager::load_urdf( std::lock_guard guard(resource_interfaces_lock_); std::lock_guard guard_claimed(claimed_command_interfaces_lock_); resource_storage_->load_and_initialize_system(individual_hardware_info); +======= + std::lock_guard resource_guard(resources_lock_); + for (const auto & individual_hardware_info : hardware_info) + { + // Check for identical names + if ( + resource_storage_->hardware_info_map_.find(individual_hardware_info.name) != + resource_storage_->hardware_info_map_.end()) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Hardware name %s is duplicated. Please provide a unique 'name' " + "in the URDF.", + individual_hardware_info.name.c_str()); + components_are_loaded_and_initialized_ = false; + break; + } + + if (individual_hardware_info.type == actuator_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + if (!resource_storage_->load_and_initialize_actuator(individual_hardware_info)) + { + components_are_loaded_and_initialized_ = false; + break; + } + } + if (individual_hardware_info.type == sensor_type) + { + std::lock_guard guard(resource_interfaces_lock_); + if (!resource_storage_->load_and_initialize_sensor(individual_hardware_info)) + { + components_are_loaded_and_initialized_ = false; + break; + } + } + if (individual_hardware_info.type == system_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + if (!resource_storage_->load_and_initialize_system(individual_hardware_info)) + { + components_are_loaded_and_initialized_ = false; + break; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } } +<<<<<<< HEAD // throw on missing state and command interfaces, not specified keys are being ignored if (validate_interfaces) { @@ -723,6 +1573,30 @@ void ResourceManager::load_urdf( } bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +======= + if (components_are_loaded_and_initialized_ && validate_storage(hardware_info)) + { + std::lock_guard guard(resources_lock_); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); + } + else + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + resource_storage_->clear(); + // cleanup everything + components_are_loaded_and_initialized_ = false; + } + + return components_are_loaded_and_initialized_; +} + +bool ResourceManager::are_components_initialized() const +{ + return components_are_loaded_and_initialized_; +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) @@ -766,11 +1640,81 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con } // CM API: Called in "callback/slow"-thread +<<<<<<< HEAD void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); std::lock_guard guard_claimed(claimed_command_interfaces_lock_); +======= +void ResourceManager::import_controller_exported_state_interfaces( + const std::string & controller_name, std::vector & interfaces) +{ + std::lock_guard guard(resource_interfaces_lock_); + auto interface_names = resource_storage_->add_state_interfaces(interfaces); + resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; +} + +// CM API: Called in "callback/slow"-thread +std::vector ResourceManager::get_controller_exported_state_interface_names( + const std::string & controller_name) +{ + return resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); +} + +// CM API: Called in "update"-thread +void ResourceManager::make_controller_exported_state_interfaces_available( + const std::string & controller_name) +{ + auto interface_names = + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->available_state_interfaces_.insert( + resource_storage_->available_state_interfaces_.end(), interface_names.begin(), + interface_names.end()); +} + +// CM API: Called in "update"-thread +void ResourceManager::make_controller_exported_state_interfaces_unavailable( + const std::string & controller_name) +{ + auto interface_names = + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + + std::lock_guard guard(resource_interfaces_lock_); + for (const auto & interface : interface_names) + { + auto found_it = std::find( + resource_storage_->available_state_interfaces_.begin(), + resource_storage_->available_state_interfaces_.end(), interface); + if (found_it != resource_storage_->available_state_interfaces_.end()) + { + resource_storage_->available_state_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "'%s' state interface removed from available list", interface.c_str()); + } + } +} + +// CM API: Called in "callback/slow"-thread +void ResourceManager::remove_controller_exported_state_interfaces( + const std::string & controller_name) +{ + auto interface_names = + resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); + resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name); + + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->remove_state_interfaces(interface_names); +} + +// CM API: Called in "callback/slow"-thread +void ResourceManager::import_controller_reference_interfaces( + const std::string & controller_name, + const std::vector & interfaces) +{ + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto interface_names = resource_storage_->add_command_interfaces(interfaces); resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } @@ -810,9 +1754,14 @@ void ResourceManager::make_controller_reference_interfaces_unavailable( if (found_it != resource_storage_->available_command_interfaces_.end()) { resource_storage_->available_command_interfaces_.erase(found_it); +<<<<<<< HEAD RCUTILS_LOG_DEBUG_NAMED( "resource_manager", "'%s' command interface removed from available list", interface.c_str()); +======= + RCLCPP_DEBUG( + get_logger(), "'%s' command interface removed from available list", interface.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } } @@ -824,8 +1773,12 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->controllers_reference_interfaces_map_.at(controller_name); resource_storage_->controllers_reference_interfaces_map_.erase(controller_name); +<<<<<<< HEAD std::lock_guard guard(resource_interfaces_lock_); std::lock_guard guard_claimed(claimed_command_interfaces_lock_); +======= + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) resource_storage_->remove_command_interfaces(interface_names); } @@ -979,6 +1932,7 @@ void ResourceManager::import_component( // CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() { +<<<<<<< HEAD for (auto & component : resource_storage_->actuators_) { resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); @@ -991,6 +1945,23 @@ std::unordered_map ResourceManager::get_comp { resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); } +======= + auto loop_and_get_state = [&](auto & container) + { + for (auto & component : container) + { + resource_storage_->hardware_info_map_[component.get_name()].state = + component.get_lifecycle_state(); + } + }; + + 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_); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return resource_storage_->hardware_info_map_; } @@ -1006,6 +1977,7 @@ bool ResourceManager::prepare_command_mode_switch( return true; } +<<<<<<< HEAD auto interfaces_to_string = [&]() { std::stringstream ss; @@ -1024,6 +1996,8 @@ bool ResourceManager::prepare_command_mode_switch( return ss.str(); }; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check if interface exists std::stringstream ss_not_existing; ss_not_existing << "Not existing: " << std::endl << "[" << std::endl; @@ -1043,9 +2017,16 @@ bool ResourceManager::prepare_command_mode_switch( if (!(check_exist(start_interfaces) && check_exist(stop_interfaces))) { ss_not_existing << "]" << std::endl; +<<<<<<< HEAD RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Not acceptable command interfaces combination: \n%s%s", interfaces_to_string().c_str(), ss_not_existing.str().c_str()); +======= + RCLCPP_ERROR( + get_logger(), "Not acceptable command interfaces combination: \n%s%s", + interfaces_to_string(start_interfaces, stop_interfaces).c_str(), + ss_not_existing.str().c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return false; } @@ -1068,19 +2049,31 @@ bool ResourceManager::prepare_command_mode_switch( if (!(check_available(start_interfaces) && check_available(stop_interfaces))) { ss_not_available << "]" << std::endl; +<<<<<<< HEAD RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Not acceptable command interfaces combination: \n%s%s", interfaces_to_string().c_str(), ss_not_available.str().c_str()); +======= + RCLCPP_ERROR( + get_logger(), "Not acceptable command interfaces combination: \n%s%s", + interfaces_to_string(start_interfaces, stop_interfaces).c_str(), + ss_not_available.str().c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return false; } auto call_prepare_mode_switch = +<<<<<<< HEAD [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components) +======= + [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { bool ret = true; for (auto & component : components) { if ( +<<<<<<< HEAD component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -1092,6 +2085,43 @@ bool ResourceManager::prepare_command_mode_switch( "resource_manager", "Component '%s' did not accept command interfaces combination: \n%s", component.get_name().c_str(), interfaces_to_string().c_str()); +======= + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + try + { + if ( + return_type::OK != + component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + { + RCLCPP_ERROR( + logger, "Component '%s' did not accept command interfaces combination: \n%s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + ret = false; + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + logger, + "Exception of type : %s occurred while preparing command mode switch for component " + "'%s' for the interfaces: \n %s : %s", + typeid(e).name(), component.get_name().c_str(), + interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); + ret = false; + } + catch (...) + { + RCLCPP_ERROR( + logger, + "Unknown exception occurred while preparing command mode switch for component '%s' for " + "the interfaces: \n %s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces, stop_interfaces).c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ret = false; } } @@ -1116,12 +2146,18 @@ bool ResourceManager::perform_command_mode_switch( return true; } +<<<<<<< HEAD auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) +======= + auto call_perform_mode_switch = + [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { bool ret = true; for (auto & component : components) { if ( +<<<<<<< HEAD component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -1132,6 +2168,42 @@ bool ResourceManager::perform_command_mode_switch( RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Component '%s' could not perform switch", component.get_name().c_str()); +======= + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + try + { + if ( + return_type::OK != + component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + { + RCLCPP_ERROR( + logger, "Component '%s' could not perform switch", component.get_name().c_str()); + ret = false; + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + logger, + "Exception of type : %s occurred while performing command mode switch for component " + "'%s' for the interfaces: \n %s : %s", + typeid(e).name(), component.get_name().c_str(), + interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); + ret = false; + } + catch (...) + { + RCLCPP_ERROR( + logger, + "Unknown exception occurred while performing command mode switch for component '%s' " + "for " + "the interfaces: \n %s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces, stop_interfaces).c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ret = false; } } @@ -1157,9 +2229,14 @@ return_type ResourceManager::set_component_state( if (found_it == resource_storage_->hardware_info_map_.end()) { +<<<<<<< HEAD RCUTILS_LOG_INFO_NAMED( "resource_manager", "Hardware Component with name '%s' does not exists", component_name.c_str()); +======= + RCLCPP_INFO( + get_logger(), "Hardware Component with name '%s' does not exists", component_name.c_str()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::ERROR; } @@ -1226,15 +2303,49 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } +<<<<<<< HEAD +======= + 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_); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return result; } +<<<<<<< HEAD +======= +void ResourceManager::shutdown_async_components() +{ + resource_storage_->async_component_threads_.erase( + resource_storage_->async_component_threads_.begin(), + resource_storage_->async_component_threads_.end()); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { +<<<<<<< HEAD std::lock_guard guard(resources_lock_); +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); @@ -1242,12 +2353,79 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { +<<<<<<< HEAD if (component.read(time, period) != return_type::OK) { +======= + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping read() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } + auto ret_val = return_type::OK; + try + { + if ( + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == + resource_storage_->cm_update_rate_) + { + ret_val = component.read(time, period); + } + else + { + const double read_rate = + resource_storage_->hardware_info_map_[component.get_name()].rw_rate; + const auto current_time = resource_storage_->get_clock()->now(); + const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); + if (actual_period.seconds() * read_rate >= 0.99) + { + ret_val = component.read(current_time, actual_period); + } + } + const auto component_group = component.get_group_name(); + ret_val = + resource_storage_->update_hardware_component_group_state(component_group, ret_val); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s thrown during read of the component '%s': %s", + typeid(e).name(), component.get_name().c_str(), e.what()); + ret_val = return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception thrown during read of the component '%s'", + component.get_name().c_str()); + ret_val = return_type::ERROR; + } + if (ret_val == return_type::ERROR) + { + component.error(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } +<<<<<<< HEAD +======= + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } + // If desired: automatic re-activation. We could add a flag for this... + // else + // { + // using lifecycle_msgs::msg::State; + // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + // set_component_state(component.get_name(), state); + // } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } }; @@ -1262,7 +2440,10 @@ HardwareReadWriteStatus ResourceManager::read( HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { +<<<<<<< HEAD std::lock_guard guard(resources_lock_); +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); @@ -1270,12 +2451,72 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { +<<<<<<< HEAD if (component.write(time, period) != return_type::OK) { +======= + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping write() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } + auto ret_val = return_type::OK; + try + { + if ( + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == + resource_storage_->cm_update_rate_) + { + ret_val = component.write(time, period); + } + else + { + const double write_rate = + resource_storage_->hardware_info_map_[component.get_name()].rw_rate; + const auto current_time = resource_storage_->get_clock()->now(); + const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); + if (actual_period.seconds() * write_rate >= 0.99) + { + ret_val = component.write(current_time, actual_period); + } + } + const auto component_group = component.get_group_name(); + ret_val = + resource_storage_->update_hardware_component_group_state(component_group, ret_val); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Exception of type : %s thrown during write of the component '%s': %s", + typeid(e).name(), component.get_name().c_str(), e.what()); + ret_val = return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Unknown exception thrown during write of the component '%s'", + component.get_name().c_str()); + ret_val = return_type::ERROR; + } + if (ret_val == return_type::ERROR) + { + component.error(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } +<<<<<<< HEAD +======= + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } }; @@ -1314,11 +2555,27 @@ bool ResourceManager::state_interface_exists(const std::string & key) const return resource_storage_->state_interface_map_.find(key) != resource_storage_->state_interface_map_.end(); } +<<<<<<< HEAD // END: "used only in tests and locally" // BEGIN: private methods void ResourceManager::validate_storage( +======= + +// END: "used only in tests and locally" + +rclcpp::Logger ResourceManager::get_logger() const { return resource_storage_->get_logger(); } + +rclcpp::Clock::SharedPtr ResourceManager::get_clock() const +{ + return resource_storage_->get_clock(); +} + +// BEGIN: private methods + +bool ResourceManager::validate_storage( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const std::vector & hardware_info) const { std::vector missing_state_keys = {}; @@ -1374,6 +2631,7 @@ void ResourceManager::validate_storage( if (!missing_state_keys.empty() || !missing_command_keys.empty()) { +<<<<<<< HEAD std::string err_msg = "Wrong state or command interface configuration.\n"; err_msg += "missing state interfaces:\n"; for (const auto & missing_key : missing_state_keys) @@ -1409,6 +2667,30 @@ void ResourceManager::activate_all_components() { set_component_state(component.get_name(), active_state); } +======= + std::string message = "Wrong state or command interface configuration.\n"; + message += "missing state interfaces:\n"; + for (const auto & missing_key : missing_state_keys) + { + message += " " + missing_key + "\t"; + } + message += "\nmissing command interfaces:\n"; + for (const auto & missing_key : missing_command_keys) + { + message += " " + missing_key + "\t"; + } + + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Discrepancy between robot description file (urdf) and actually exported HW interfaces.\n" + "Details: %s", + message.c_str()); + + return false; + } + + return true; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // END: private methods diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 2e53e447b9..1b4ed718ce 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -21,10 +21,18 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD +======= +#include "hardware_interface/lifecycle_helpers.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +<<<<<<< HEAD +======= +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -34,6 +42,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} +<<<<<<< HEAD const rclcpp_lifecycle::State & Sensor::initialize(const HardwareInfo & sensor_info) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) @@ -42,21 +51,51 @@ const rclcpp_lifecycle::State & Sensor::initialize(const HardwareInfo & sensor_i { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= +Sensor::Sensor(Sensor && other) noexcept +{ + std::lock_guard lock(other.sensors_mutex_); + impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; +} + +const rclcpp_lifecycle::State & Sensor::initialize( + const HardwareInfo & sensor_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + std::unique_lock lock(sensors_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->init(sensor_info, logger, clock_interface)) + { + case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(rclcpp_lifecycle::State( +======= + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } +<<<<<<< HEAD return impl_->get_state(); +======= + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Sensor::configure() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -67,39 +106,79 @@ const rclcpp_lifecycle::State & Sensor::configure() break; case CallbackReturn::FAILURE: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(sensors_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Sensor::cleanup() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(sensors_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Sensor::shutdown() { +<<<<<<< HEAD if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -108,19 +187,39 @@ const rclcpp_lifecycle::State & Sensor::shutdown() { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(sensors_mutex_); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Sensor::activate() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -139,10 +238,32 @@ const rclcpp_lifecycle::State & Sensor::activate() } } return impl_->get_state(); +======= + std::unique_lock lock(sensors_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_activate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Sensor::deactivate() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -161,36 +282,106 @@ const rclcpp_lifecycle::State & Sensor::deactivate() } } return impl_->get_state(); +======= + std::unique_lock lock(sensors_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & Sensor::error() { +<<<<<<< HEAD if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(sensors_mutex_); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_error(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(rclcpp_lifecycle::State( +======= + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } +<<<<<<< HEAD return impl_->get_state(); } std::vector Sensor::export_state_interfaces() { return impl_->export_state_interfaces(); +======= + return impl_->get_lifecycle_state(); +} + +std::vector Sensor::export_state_interfaces() +{ + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } std::string Sensor::get_name() const { return impl_->get_name(); } +<<<<<<< HEAD const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -199,20 +390,49 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { +======= +std::string Sensor::get_group_name() const { return impl_->get_group_name(); } + +const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} + +const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } + +return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_read_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } return_type result = return_type::ERROR; if ( +<<<<<<< HEAD impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { result = impl_->read(time, period); if (result == return_type::ERROR) { error(); } +<<<<<<< HEAD +======= + last_read_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } +<<<<<<< HEAD +======= +std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ee942d6581..d8369fcc78 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -21,6 +21,10 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD +======= +#include "hardware_interface/lifecycle_helpers.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -34,6 +38,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} +<<<<<<< HEAD const rclcpp_lifecycle::State & System::initialize(const HardwareInfo & system_info) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) @@ -42,21 +47,53 @@ const rclcpp_lifecycle::State & System::initialize(const HardwareInfo & system_i { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= +System::System(System && other) noexcept +{ + std::lock_guard lock(other.system_mutex_); + impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; + last_write_cycle_time_ = other.last_write_cycle_time_; +} + +const rclcpp_lifecycle::State & System::initialize( + const HardwareInfo & system_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + std::unique_lock lock(system_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->init(system_info, logger, clock_interface)) + { + case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_write_cycle_time_ = clock_interface->get_clock()->now(); + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(rclcpp_lifecycle::State( +======= + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } +<<<<<<< HEAD return impl_->get_state(); +======= + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & System::configure() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -67,39 +104,79 @@ const rclcpp_lifecycle::State & System::configure() break; case CallbackReturn::FAILURE: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(system_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & System::cleanup() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(system_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & System::shutdown() { +<<<<<<< HEAD if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -108,19 +185,39 @@ const rclcpp_lifecycle::State & System::shutdown() { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(system_mutex_); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(error()); break; } } return impl_->get_state(); +======= + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & System::activate() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -139,10 +236,32 @@ const rclcpp_lifecycle::State & System::activate() } } return impl_->get_state(); +======= + std::unique_lock lock(system_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_activate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & System::deactivate() { +<<<<<<< HEAD if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -161,26 +280,64 @@ const rclcpp_lifecycle::State & System::deactivate() } } return impl_->get_state(); +======= + std::unique_lock lock(system_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } const rclcpp_lifecycle::State & System::error() { +<<<<<<< HEAD if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( +======= + std::unique_lock lock(system_mutex_); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_error(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: +<<<<<<< HEAD impl_->set_state(rclcpp_lifecycle::State( +======= + impl_->set_lifecycle_state(rclcpp_lifecycle::State( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } +<<<<<<< HEAD return impl_->get_state(); } @@ -192,6 +349,64 @@ std::vector System::export_state_interfaces() std::vector System::export_command_interfaces() { return impl_->export_command_interfaces(); +======= + return impl_->get_lifecycle_state(); +} + +std::vector System::export_state_interfaces() +{ + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility +} + +std::vector System::export_command_interfaces() +{ + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_command_interfaces(); + // END: for backward compatibility + + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return_type System::prepare_command_mode_switch( @@ -210,6 +425,7 @@ return_type System::perform_command_mode_switch( std::string System::get_name() const { return impl_->get_name(); } +<<<<<<< HEAD const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); } return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -218,42 +434,88 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { +======= +std::string System::get_group_name() const { return impl_->get_group_name(); } + +const rclcpp_lifecycle::State & System::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} + +const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } + +return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_read_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } return_type result = return_type::ERROR; if ( +<<<<<<< HEAD impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { result = impl_->read(time, period); if (result == return_type::ERROR) { error(); } +<<<<<<< HEAD +======= + last_read_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { +<<<<<<< HEAD if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { +======= + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_write_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } return_type result = return_type::ERROR; if ( +<<<<<<< HEAD impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +======= + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { result = impl_->write(time, period); if (result == return_type::ERROR) { error(); } +<<<<<<< HEAD +======= + last_write_cycle_time_ = time; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return result; } +<<<<<<< HEAD +======= +std::recursive_mutex & System::get_mutex() { return system_mutex_; } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace hardware_interface diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 41ce1d9041..13557a5c67 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -26,8 +26,13 @@ #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +<<<<<<< HEAD #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" +======= +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/state.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "ros2_control_test_assets/descriptions.hpp" namespace @@ -40,13 +45,22 @@ const auto COMPARE_DELTA = 0.0001; class TestGenericSystem : public ::testing::Test { public: +<<<<<<< HEAD void test_generic_system_with_mimic_joint(const std::string & urdf); void test_generic_system_with_mock_sensor_commands(const std::string & urdf); void test_generic_system_with_mock_gpio_commands(const std::string & urdf); +======= + void test_generic_system_with_mimic_joint(std::string & urdf, const std::string & component_name); + void test_generic_system_with_mock_sensor_commands( + std::string & urdf, const std::string & component_name); + void test_generic_system_with_mock_gpio_commands( + std::string & urdf, const std::string & component_name); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) protected: void SetUp() override { +<<<<<<< HEAD // REMOVE THIS MEMBER ONCE FAKE COMPONENTS ARE REMOVED hardware_fake_system_2dof_ = R"( @@ -70,11 +84,17 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_ = R"( +======= + hardware_system_2dof_ = + R"( + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem +<<<<<<< HEAD 1.57 @@ -82,18 +102,34 @@ class TestGenericSystem : public ::testing::Test 0.7854 +======= + + 1.57 + + + + + + 0.7854 + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; hardware_system_2dof_asymetric_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem +<<<<<<< HEAD 1.57 @@ -102,50 +138,99 @@ class TestGenericSystem : public ::testing::Test 0.7854 0.8554 +======= + + 1.57 + + + + + + 0.7854 + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; hardware_system_2dof_standard_interfaces_ = R"( +<<<<<<< HEAD mock_components/GenericSystem +======= + + + mock_components/GenericSystem + Hardware Group +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 3.45 +======= + + 3.45 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 2.78 +======= + + 2.78 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; hardware_system_2dof_with_other_interface_ = R"( +<<<<<<< HEAD mock_components/GenericSystem +======= + + + mock_components/GenericSystem + Hardware Group +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 1.55 0.1 +======= + + 1.55 + + + 0.1 + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 0.65 @@ -156,12 +241,31 @@ class TestGenericSystem : public ::testing::Test 0.5 +======= + + 0.65 + + + 0.2 + + + + + + 0.5 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; hardware_system_2dof_with_sensor_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem @@ -189,7 +293,11 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem true @@ -218,10 +326,17 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_True_ = R"( +<<<<<<< HEAD mock_components/GenericSystem True +======= + + + mock_components/GenericSystem + True +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -247,13 +362,18 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_mimic_joint_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem +<<<<<<< HEAD 1.57 @@ -263,6 +383,14 @@ class TestGenericSystem : public ::testing::Test -2 +======= + + 1.57 + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -271,7 +399,11 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_offset_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem -3 @@ -297,32 +429,61 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ = R"( +<<<<<<< HEAD mock_components/GenericSystem +======= + + + mock_components/GenericSystem + Hardware Group +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) -3 actual_position +<<<<<<< HEAD 3.45 +======= + + 3.45 + + + 0.0 + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 2.78 +======= + + 2.78 + + + 0.0 + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem -3 @@ -331,27 +492,52 @@ class TestGenericSystem : public ::testing::Test +<<<<<<< HEAD 3.45 +======= + + 3.45 + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 2.78 +======= + + 2.78 + + + 0.0 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; valid_urdf_ros2_control_system_robot_with_gpio_ = R"( +<<<<<<< HEAD mock_components/GenericSystem +======= + + + mock_components/GenericSystem + Hardware Group +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2 2 @@ -366,9 +552,16 @@ class TestGenericSystem : public ::testing::Test +<<<<<<< HEAD 2.78 +======= + + 2.78 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -385,7 +578,11 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem true @@ -393,16 +590,30 @@ class TestGenericSystem : public ::testing::Test +<<<<<<< HEAD 3.45 +======= + + 3.45 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 2.78 +======= + + 2.78 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -419,24 +630,45 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( +<<<<<<< HEAD mock_components/GenericSystem True +======= + + + mock_components/GenericSystem + True +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 3.45 +======= + + 3.45 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD 2.78 +======= + + 2.78 + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -453,9 +685,15 @@ class TestGenericSystem : public ::testing::Test sensor_with_initial_value_ = R"( +<<<<<<< HEAD fake_components/GenericSystem +======= + + + mock_components/GenericSystem +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -473,9 +711,15 @@ class TestGenericSystem : public ::testing::Test gpio_with_initial_value_ = R"( +<<<<<<< HEAD fake_components/GenericSystem +======= + + + mock_components/GenericSystem +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -487,7 +731,11 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem true @@ -507,7 +755,10 @@ class TestGenericSystem : public ::testing::Test 2.78 +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -520,7 +771,11 @@ class TestGenericSystem : public ::testing::Test valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) mock_components/GenericSystem true @@ -538,7 +793,10 @@ class TestGenericSystem : public ::testing::Test 2.78 +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -547,7 +805,10 @@ class TestGenericSystem : public ::testing::Test 2.78 +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -560,9 +821,15 @@ class TestGenericSystem : public ::testing::Test disabled_commands_ = R"( +<<<<<<< HEAD fake_components/GenericSystem +======= + + + mock_components/GenericSystem +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) True @@ -575,11 +842,81 @@ class TestGenericSystem : public ::testing::Test )"; +<<<<<<< HEAD } std::string hardware_robot_2dof_; std::string hardware_system_2dof_; std::string hardware_fake_system_2dof_; +======= + + hardware_system_2dof_standard_interfaces_with_same_hardware_group_ = + R"( + + + mock_components/GenericSystem + Hardware Group + + + + + + 3.45 + + + + + + + mock_components/GenericSystem + Hardware Group + + + + + + 2.78 + + + + +)"; + + hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ = + R"( + + + mock_components/GenericSystem + Hardware Group 1 + + + + + + 3.45 + + + + + + + mock_components/GenericSystem + Hardware Group 2 + + + + + + 2.78 + + + + +)"; + } + + std::string hardware_system_2dof_; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::string hardware_system_2dof_asymetric_; std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; @@ -598,6 +935,12 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; +<<<<<<< HEAD +======= + std::string hardware_system_2dof_standard_interfaces_with_same_hardware_group_; + std::string hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_; + rclcpp::Node node_ = rclcpp::Node("TestGenericSystem"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; // Forward declaration @@ -622,11 +965,24 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command); FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True); +<<<<<<< HEAD TestableResourceManager() : hardware_interface::ResourceManager() {} TestableResourceManager( const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) +======= + explicit TestableResourceManager(rclcpp::Node & node) + : hardware_interface::ResourceManager( + node.get_node_clock_interface(), node.get_node_logging_interface()) + { + } + + explicit TestableResourceManager( + rclcpp::Node & node, const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager( + urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } }; @@ -673,6 +1029,7 @@ TEST_F(TestGenericSystem, load_generic_system_2dof) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD ASSERT_NO_THROW(TestableResourceManager rm(urdf)); } @@ -705,6 +1062,9 @@ TEST_F(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces) ASSERT_EQ(0.7854, j2p_s.get_value()); ASSERT_TRUE(std::isnan(j1p_c.get_value())); ASSERT_TRUE(std::isnan(j2p_c.get_value())); +======= + ASSERT_NO_THROW(TestableResourceManager rm(node_, urdf)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // Test inspired by hardware_interface/test_resource_manager.cpp @@ -712,9 +1072,15 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -743,9 +1109,15 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -781,12 +1153,17 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); +<<<<<<< HEAD ASSERT_EQ(0.0, j1v_s.get_value()); +======= + ASSERT_EQ(1.57, j1v_s.get_value()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(0.7854, j2p_s.get_value()); ASSERT_TRUE(std::isnan(j1p_c.get_value())); ASSERT_TRUE(std::isnan(j2a_c.get_value())); } +<<<<<<< HEAD void generic_system_functional_test(const std::string & urdf, const double offset = 0) { TestableResourceManager rm(urdf); @@ -805,6 +1182,27 @@ void generic_system_functional_test(const std::string & urdf, const double offse EXPECT_EQ( status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +======= +void generic_system_functional_test( + const std::string & urdf, const std::string component_name = "GenericSystem2dof", + const double offset = 0) +{ + rclcpp::Node node("test_generic_system"); + TestableResourceManager rm(node, urdf); + // check is hardware is configured + auto status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component_name].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); + configure_components(rm, {component_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {component_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -843,7 +1241,11 @@ void generic_system_functional_test(const std::string & urdf, const double offse ASSERT_EQ(0.44, j2v_c.get_value()); // write() does not change values +<<<<<<< HEAD rm.write(TIME, PERIOD); +======= + ASSERT_TRUE(rm.write(TIME, PERIOD).ok); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(3.45, j1p_s.get_value()); ASSERT_EQ(0.0, j1v_s.get_value()); ASSERT_EQ(2.78, j2p_s.get_value()); @@ -854,7 +1256,11 @@ void generic_system_functional_test(const std::string & urdf, const double offse ASSERT_EQ(0.44, j2v_c.get_value()); // read() mirrors commands + offset to states +<<<<<<< HEAD rm.read(TIME, PERIOD); +======= + ASSERT_TRUE(rm.read(TIME, PERIOD).ok); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(0.11 + offset, j1p_s.get_value()); ASSERT_EQ(0.22, j1v_s.get_value()); ASSERT_EQ(0.33 + offset, j2p_s.get_value()); @@ -880,11 +1286,171 @@ void generic_system_functional_test(const std::string & urdf, const double offse ASSERT_EQ(0.77, j2p_c.get_value()); ASSERT_EQ(0.88, j2v_c.get_value()); +<<<<<<< HEAD deactivate_components(rm); status_map = rm.get_components_status(); EXPECT_EQ( status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +======= + deactivate_components(rm, {component_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +} + +void generic_system_error_group_test( + const std::string & urdf, const std::string component_prefix, bool validate_same_group) +{ + rclcpp::Node node("test_generic_system"); + TestableResourceManager rm(node, urdf); + const std::string component1 = component_prefix + "1"; + const std::string component2 = component_prefix + "2"; + // check is hardware is configured + auto status_map = rm.get_components_status(); + for (auto component : {component1, component2}) + { + EXPECT_EQ( + status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); + configure_components(rm, {component}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {component}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + } + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + + // State interfaces without initial value are set to 0 + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + + // set some new values in commands + j1p_c.set_value(0.11); + j1v_c.set_value(0.22); + j2p_c.set_value(0.33); + j2v_c.set_value(0.44); + + // State values should not be changed + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); + + // write() does not change values + ASSERT_TRUE(rm.write(TIME, PERIOD).ok); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); + + // read() mirrors commands to states + ASSERT_TRUE(rm.read(TIME, PERIOD).ok); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); + + // set some new values in commands + j1p_c.set_value(0.55); + j1v_c.set_value(0.66); + j2p_c.set_value(0.77); + j2v_c.set_value(0.88); + + // state values should not be changed + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.55, j1p_c.get_value()); + ASSERT_EQ(0.66, j1v_c.get_value()); + ASSERT_EQ(0.77, j2p_c.get_value()); + ASSERT_EQ(0.88, j2v_c.get_value()); + + // Error testing + j1p_c.set_value(std::numeric_limits::infinity()); + j1v_c.set_value(std::numeric_limits::infinity()); + // read() should now bring error in the first component + auto read_result = rm.read(TIME, PERIOD); + ASSERT_FALSE(read_result.ok); + if (validate_same_group) + { + // If they belong to the same group, show the error in all hardware components of same group + EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1, component2)); + } + else + { + // If they don't belong to the same group, show the error in only that hardware component + EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1)); + } + + // Check initial values + ASSERT_FALSE(rm.state_interface_is_available("joint1/position")); + ASSERT_FALSE(rm.state_interface_is_available("joint1/velocity")); + ASSERT_FALSE(rm.command_interface_is_available("joint1/position")); + ASSERT_FALSE(rm.command_interface_is_available("joint1/velocity")); + + if (validate_same_group) + { + ASSERT_FALSE(rm.state_interface_is_available("joint2/position")); + ASSERT_FALSE(rm.state_interface_is_available("joint2/velocity")); + ASSERT_FALSE(rm.command_interface_is_available("joint2/position")); + ASSERT_FALSE(rm.command_interface_is_available("joint2/velocity")); + } + else + { + ASSERT_TRUE(rm.state_interface_is_available("joint2/position")); + ASSERT_TRUE(rm.state_interface_is_available("joint2/velocity")); + ASSERT_TRUE(rm.command_interface_is_available("joint2/position")); + ASSERT_TRUE(rm.command_interface_is_available("joint2/velocity")); + } + + // Error should be recoverable only after reactivating the hardware component + j1p_c.set_value(0.0); + j1v_c.set_value(0.0); + ASSERT_FALSE(rm.read(TIME, PERIOD).ok); + + // Now it should be recoverable + deactivate_components(rm, {component1}); + activate_components(rm, {component1}); + ASSERT_TRUE(rm.read(TIME, PERIOD).ok); + + deactivate_components(rm, {component1, component2}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component1].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ( + status_map[component2].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, generic_system_2dof_functionality) @@ -892,16 +1458,44 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD generic_system_functional_test(urdf); +======= + generic_system_functional_test(urdf, {"MockHardwareSystem"}); +} + +TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_different_group) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ + + ros2_control_test_assets::urdf_tail; + + generic_system_error_group_test(urdf, {"MockHardwareSystem"}, false); +} + +TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_same_group) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_same_hardware_group_ + + ros2_control_test_assets::urdf_tail; + + generic_system_error_group_test(urdf, {"MockHardwareSystem"}, true); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -982,9 +1576,15 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1078,11 +1678,20 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_EQ(0.33, j2p_c.get_value()); } +<<<<<<< HEAD void TestGenericSystem::test_generic_system_with_mock_sensor_commands(const std::string & urdf) { TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= +void TestGenericSystem::test_generic_system_with_mock_sensor_commands( + std::string & urdf, const std::string & component_name) +{ + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {component_name}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1205,7 +1814,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD test_generic_system_with_mock_sensor_commands(urdf); +======= + test_generic_system_with_mock_sensor_commands(urdf, "MockHardwareSystem"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) @@ -1214,6 +1827,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD test_generic_system_with_mock_sensor_commands(urdf); } @@ -1222,6 +1836,17 @@ void TestGenericSystem::test_generic_system_with_mimic_joint(const std::string & TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + test_generic_system_with_mock_sensor_commands(urdf, "MockHardwareSystem"); +} + +void TestGenericSystem::test_generic_system_with_mimic_joint( + std::string & urdf, const std::string & component_name) +{ + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {component_name}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1231,11 +1856,17 @@ void TestGenericSystem::test_generic_system_with_mimic_joint(const std::string & EXPECT_TRUE(rm.state_interface_exists("joint2/position")); EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); +<<<<<<< HEAD ASSERT_EQ(4u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint2/position")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); +======= + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -1285,10 +1916,17 @@ void TestGenericSystem::test_generic_system_with_mimic_joint(const std::string & TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) { +<<<<<<< HEAD auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mimic_joint(urdf); +======= + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mimic_joint(urdf, "MockHardwareSystem"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) @@ -1297,7 +1935,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) hardware_system_2dof_standard_interfaces_with_offset_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD generic_system_functional_test(urdf, -3); +======= + generic_system_functional_test(urdf, "MockHardwareSystem", -3); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface_missing) @@ -1307,7 +1949,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ros2_control_test_assets::urdf_tail; // custom interface is missing so offset will not be applied +<<<<<<< HEAD generic_system_functional_test(urdf, 0.0); +======= + generic_system_functional_test(urdf, "MockHardwareSystem", 0.0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface) @@ -1318,13 +1964,20 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i const double offset = -3; +<<<<<<< HEAD TestableResourceManager rm(urdf); const std::string hardware_name = "GenericSystem2dof"; +======= + TestableResourceManager rm(node_, urdf); + + const std::string hardware_name = "MockHardwareSystem"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // check is hardware is configured auto status_map = rm.get_components_status(); EXPECT_EQ( +<<<<<<< HEAD status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); @@ -1338,6 +1991,19 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i EXPECT_EQ( status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +======= + status_map[hardware_name].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); + + configure_components(rm, {hardware_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {hardware_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -1423,24 +2089,38 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.77, j2p_c.get_value()); ASSERT_EQ(0.88, j2v_c.get_value()); +<<<<<<< HEAD deactivate_components(rm); status_map = rm.get_components_status(); EXPECT_EQ( status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +======= + deactivate_components(rm, {hardware_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) { auto urdf = ros2_control_test_assets::urdf_head + valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); const std::string hardware_name = "GenericSystem2dof"; +======= + TestableResourceManager rm(node_, urdf); + + const std::string hardware_name = "MockHardwareSystem"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // check is hardware is started auto status_map = rm.get_components_status(); EXPECT_EQ( +<<<<<<< HEAD status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); configure_components(rm); @@ -1453,6 +2133,18 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) EXPECT_EQ( status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +======= + status_map[hardware_name].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); + configure_components(rm, {hardware_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {hardware_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(8u, rm.state_interface_keys().size()); ASSERT_EQ(6u, rm.command_interface_keys().size()); @@ -1527,16 +2219,27 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_EQ(0.444, gpio2_vac_c.get_value()); // check other functionalities are working well +<<<<<<< HEAD generic_system_functional_test(urdf); } void TestGenericSystem::test_generic_system_with_mock_gpio_commands(const std::string & urdf) { TestableResourceManager rm(urdf); +======= + generic_system_functional_test(urdf, hardware_name); +} + +void TestGenericSystem::test_generic_system_with_mock_gpio_commands( + std::string & urdf, const std::string & component_name) +{ + TestableResourceManager rm(node_, urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // check is hardware is started auto status_map = rm.get_components_status(); EXPECT_EQ( +<<<<<<< HEAD status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); configure_components(rm); @@ -1549,6 +2252,18 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands(const std::s EXPECT_EQ( status_map["GenericSystem2dof"].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +======= + status_map[component_name].state.label(), + hardware_interface::lifecycle_state_names::UNCONFIGURED); + configure_components(rm, {component_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {component_name}); + status_map = rm.get_components_status(); + EXPECT_EQ( + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1644,7 +2359,11 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD test_generic_system_with_mock_gpio_commands(urdf); +======= + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) @@ -1653,16 +2372,26 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD test_generic_system_with_mock_gpio_commands(urdf); +======= + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestGenericSystem, sensor_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1688,9 +2417,15 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1709,9 +2444,15 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) hardware_system_2dof_standard_interfaces_with_different_control_modes_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1903,9 +2644,15 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) { auto urdf = ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; +<<<<<<< HEAD TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); +======= + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1948,12 +2695,22 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) { +<<<<<<< HEAD auto check_prepare_command_mode_switch = [&](const std::string & urdf) { TestableResourceManager rm( ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); rm.set_component_state("GenericSystem2dof", state); +======= + auto check_prepare_command_mode_switch = + [&]( + const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head) + { + TestableResourceManager rm(node_, urdf_head + urdf + ros2_control_test_assets::urdf_tail); + rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + rm.set_component_state("MockHardwareSystem", state); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto start_interfaces = rm.command_interface_keys(); std::vector stop_interfaces; return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); @@ -1967,7 +2724,12 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); ASSERT_TRUE( check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); +<<<<<<< HEAD ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_)); +======= + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_with_mimic_joint_, ros2_control_test_assets::urdf_head_mimic)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_TRUE( check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); ASSERT_TRUE(check_prepare_command_mode_switch( @@ -1987,3 +2749,13 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_)); ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_)); } +<<<<<<< HEAD +======= + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 6028b2f4cf..66ed0b8def 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -18,6 +18,10 @@ #include #include #include +<<<<<<< HEAD +======= +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include @@ -32,7 +36,15 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +<<<<<<< HEAD #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +======= +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Values to send over command interface to trigger error in write and read methods @@ -49,6 +61,10 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class DummyActuator : public hardware_interface::ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override @@ -155,7 +171,100 @@ class DummyActuator : public hardware_interface::ActuatorInterface unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; +<<<<<<< HEAD + +======= +// END + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class DummySensor : public hardware_interface::SensorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override @@ -176,7 +285,11 @@ class DummySensor : public hardware_interface::SensorInterface // We can read some voltage level std::vector state_interfaces; state_interfaces.emplace_back( +<<<<<<< HEAD hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); +======= + hardware_interface::StateInterface("sens1", "voltage", &voltage_level_)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return state_interfaces; } @@ -219,7 +332,134 @@ class DummySensor : public hardware_interface::SensorInterface int read_calls_ = 0; bool recoverable_error_happened_ = false; }; +<<<<<<< HEAD + +======= +// END + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("sens1/voltage", 0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("sens1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySensorJointDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 10.0); + set_state("sens1/voltage", 0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorJointDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/position", position_hw_value_); + set_state("sens1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double position_hw_value_ = 0x777; + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class DummySystem : public hardware_interface::SystemInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override @@ -341,11 +581,19 @@ class DummySystem : public hardware_interface::SystemInterface private: std::array position_state_ = { +<<<<<<< HEAD std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; std::array velocity_state_ = { std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; +======= + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; + std::array velocity_state_ = { + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::array velocity_command_ = {{0.0, 0.0, 0.0}}; // Helper variables to initiate error on read @@ -353,84 +601,207 @@ class DummySystem : public hardware_interface::SystemInterface unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; +<<<<<<< HEAD +======= +// END -class DummySystemPreparePerform : public hardware_interface::SystemInterface +class DummySystemDefault : public hardware_interface::SystemInterface { - // Override the pure virtual functions with default behavior - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { - // We hardcode the info + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override { return {}; } - - std::vector export_command_interfaces() override + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - return {}; + for (auto i = 0ul; i < 3; ++i) + { + set_state(position_states_[i], 0.0); + set_state(velocity_states_[i], 0.0); + } + // reset command only if error is initiated + if (recoverable_error_happened_) + { + for (auto i = 0ul; i < 3; ++i) + { + set_command(velocity_commands_[i], 0.0); + } + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; } - std::string get_name() const override { return "DummySystemPreparePerform"; } + std::string get_name() const override { return "DummySystemDefault"; } hardware_interface::return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. return hardware_interface::return_type::OK; } hardware_interface::return_type write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - return hardware_interface::return_type::OK; - } - - // Custom prepare/perform functions - hardware_interface::return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override - { - // Criteria to test against - if (start_interfaces.size() != 1) + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { return hardware_interface::return_type::ERROR; } - if (stop_interfaces.size() != 2) + + for (size_t i = 0; i < 3; ++i) { - return hardware_interface::return_type::ERROR; + auto current_pos = get_state(position_states_[i]); + set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); + set_state(velocity_states_[i], get_command(velocity_commands_[i])); } return hardware_interface::return_type::OK; } - hardware_interface::return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override { - // Criteria to test against - if (start_interfaces.size() != 1) + for (const auto & velocity_state : velocity_states_) { - return hardware_interface::return_type::ERROR; + set_state(velocity_state, 0.0); } - if (stop_interfaces.size() != 2) + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) { - return hardware_interface::return_type::ERROR; + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; } - return hardware_interface::return_type::OK; + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; } -}; -} // namespace test_components +private: + std::vector position_states_ = { + "joint1/position", "joint2/position", "joint3/position"}; + std::vector velocity_states_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + std::vector velocity_commands_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; -TEST(TestComponentInterfaces, dummy_actuator) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + +class DummySystemPreparePerform : public hardware_interface::SystemInterface +{ + // Override the pure virtual functions with default behavior + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override + { + // We hardcode the info + return CallbackReturn::SUCCESS; + } + +<<<<<<< HEAD + std::vector export_state_interfaces() override { return {}; } + + std::vector export_command_interfaces() override + { + return {}; + } + +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + std::string get_name() const override { return "DummySystemPreparePerform"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + // Custom prepare/perform functions + hardware_interface::return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + // Criteria to test against + if (start_interfaces.size() != 1) + { + return hardware_interface::return_type::ERROR; + } + if (stop_interfaces.size() != 2) + { + return hardware_interface::return_type::ERROR; + } + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + // Criteria to test against + if (start_interfaces.size() != 1) + { + return hardware_interface::return_type::ERROR; + } + if (stop_interfaces.size() != 2) + { + return hardware_interface::return_type::ERROR; + } + return hardware_interface::return_type::OK; + } +}; + +} // namespace test_components + +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +TEST(TestComponentInterfaces, dummy_actuator) +{ + hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = actuator_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); +<<<<<<< HEAD EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); @@ -446,6 +817,23 @@ TEST(TestComponentInterfaces, dummy_actuator) double velocity_value = 1.0; command_interfaces[0].set_value(velocity_value); // velocity +======= + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0]->set_value(velocity_value); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -453,8 +841,13 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); +<<<<<<< HEAD ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity +======= + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -468,8 +861,13 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); +<<<<<<< HEAD EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity +======= + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -483,8 +881,13 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); +<<<<<<< HEAD EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity +======= + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -498,8 +901,135 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); +<<<<<<< HEAD EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity +======= + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); +} +// END + +TEST(TestComponentInterfaces, dummy_actuator_default) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + double velocity_value = 1.0; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto si_joint1_vel = test_components::vector_contains(state_interfaces, "joint1/velocity").second; + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -510,17 +1040,28 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST(TestComponentInterfaces, dummy_sensor) { hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = sensor_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); + auto state = + sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); +<<<<<<< HEAD EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()); EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); @@ -529,11 +1070,22 @@ TEST(TestComponentInterfaces, dummy_sensor) // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); +======= + EXPECT_EQ("sens1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); +<<<<<<< HEAD EXPECT_EQ(0.0, state_interfaces[0].get_value()); // It can read now @@ -541,17 +1093,134 @@ TEST(TestComponentInterfaces, dummy_sensor) EXPECT_EQ(0x666, state_interfaces[0].get_value()); } +======= + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); +} +// END + +TEST(TestComponentInterfaces, dummy_sensor_default) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(1u, state_interfaces.size()); + { + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + + // Not updated because is is UNCONFIGURED + auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_joint) +{ + hardware_interface::Sensor sensor_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_joint_voltage_sensor + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo sensor_res = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(sensor_res, node->get_logger(), node->get_node_clock_interface()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + + auto [contains_sens1_vol, si_sens1_vol] = + test_components::vector_contains(state_interfaces, "sens1/voltage"); + ASSERT_TRUE(contains_sens1_vol); + EXPECT_EQ("sens1/voltage", state_interfaces[si_sens1_vol]->get_name()); + EXPECT_EQ("voltage", state_interfaces[si_sens1_vol]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[si_sens1_vol]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + + auto [contains_joint1_pos, si_joint1_pos] = + test_components::vector_contains(state_interfaces, "joint1/position"); + ASSERT_TRUE(contains_joint1_pos); + EXPECT_EQ("joint1/position", state_interfaces[si_joint1_pos]->get_name()); + EXPECT_EQ("position", state_interfaces[si_joint1_pos]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[si_joint1_pos]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value()); +} + +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST(TestComponentInterfaces, dummy_system) { hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = system_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); +<<<<<<< HEAD EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); @@ -587,6 +1256,43 @@ TEST(TestComponentInterfaces, dummy_system) command_interfaces[0].set_value(velocity_value); // velocity command_interfaces[1].set_value(velocity_value); // velocity command_interfaces[2].set_value(velocity_value); // velocity +======= + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); + EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0]->set_value(velocity_value); // velocity + command_interfaces[1]->set_value(velocity_value); // velocity + command_interfaces[2]->set_value(velocity_value); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -594,12 +1300,21 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); +<<<<<<< HEAD ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity ASSERT_TRUE(std::isnan(state_interfaces[2].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[3].get_value())); // velocity ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity +======= + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -613,12 +1328,21 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); +<<<<<<< HEAD EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity EXPECT_EQ(step * velocity_value, state_interfaces[2].get_value()); // position value EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3].get_value()); // velocity EXPECT_EQ(step * velocity_value, state_interfaces[4].get_value()); // position value EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5].get_value()); // velocity +======= + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -632,76 +1356,467 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); +<<<<<<< HEAD EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2].get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[3].get_value()); // velocity EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4].get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[5].get_value()); // velocity +======= + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } - state = system_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + state = system_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + +<<<<<<< HEAD + EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2].get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity +======= + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); +} +// END + +TEST(TestComponentInterfaces, dummy_system_default) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(6u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } + + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + auto ci_joint2_vel = + test_components::vector_contains(command_interfaces, "joint2/velocity").second; + auto ci_joint3_vel = + test_components::vector_contains(command_interfaces, "joint3/velocity").second; + double velocity_value = 1.0; + command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + command_interfaces[ci_joint2_vel]->set_value(velocity_value); // velocity + command_interfaces[ci_joint3_vel]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto si_joint1_vel = test_components::vector_contains(state_interfaces, "joint1/velocity").second; + auto si_joint2_pos = test_components::vector_contains(state_interfaces, "joint2/position").second; + auto si_joint2_vel = test_components::vector_contains(state_interfaces, "joint2/velocity").second; + auto si_joint3_pos = test_components::vector_contains(state_interfaces, "joint3/position").second; + auto si_joint3_vel = test_components::vector_contains(state_interfaces, "joint3/velocity").second; + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); +} + +TEST(TestComponentInterfaces, dummy_command_mode_system) +{ + hardware_interface::System system_hw( + std::make_unique()); + hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD + auto state = system_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + std::vector one_key = {"joint1/position"}; + std::vector two_keys = {"joint1/position", "joint1/velocity"}; + + // Only calls with (one_key, two_keys) should return OK + EXPECT_EQ( + hardware_interface::return_type::ERROR, + system_hw.prepare_command_mode_switch(one_key, one_key)); + EXPECT_EQ( + hardware_interface::return_type::ERROR, + system_hw.perform_command_mode_switch(one_key, one_key)); + EXPECT_EQ( + hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch(one_key, two_keys)); + EXPECT_EQ( + hardware_interface::return_type::OK, system_hw.perform_command_mode_switch(one_key, two_keys)); + EXPECT_EQ( + hardware_interface::return_type::ERROR, + system_hw.prepare_command_mode_switch(two_keys, one_key)); + EXPECT_EQ( + hardware_interface::return_type::ERROR, + system_hw.perform_command_mode_switch(two_keys, one_key)); +} + +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD + auto state = actuator_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + +<<<<<<< HEAD + state = actuator_hw.get_state(); +======= + state = actuator_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); +<<<<<<< HEAD + EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(command_interfaces[0].get_value(), 0.0); +======= + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + +<<<<<<< HEAD + state = actuator_hw.get_state(); +======= + state = actuator_hw.get_lifecycle_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3].get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); -} - -TEST(TestComponentInterfaces, dummy_command_mode_system) -{ - hardware_interface::System system_hw( - std::make_unique()); - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - std::vector one_key = {"joint1/position"}; - std::vector two_keys = {"joint1/position", "joint1/velocity"}; + // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); - // Only calls with (one_key, two_keys) should return OK - EXPECT_EQ( - hardware_interface::return_type::ERROR, - system_hw.prepare_command_mode_switch(one_key, one_key)); - EXPECT_EQ( - hardware_interface::return_type::ERROR, - system_hw.perform_command_mode_switch(one_key, one_key)); - EXPECT_EQ( - hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch(one_key, two_keys)); - EXPECT_EQ( - hardware_interface::return_type::OK, system_hw.perform_command_mode_switch(one_key, two_keys)); - EXPECT_EQ( - hardware_interface::return_type::ERROR, - system_hw.prepare_command_mode_switch(two_keys, one_key)); - EXPECT_EQ( - hardware_interface::return_type::ERROR, - system_hw.perform_command_mode_switch(two_keys, one_key)); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + state = actuator_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) { hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = actuator_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -718,18 +1833,27 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) // Initiate error on write (this is first time therefore recoverable) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); +<<<<<<< HEAD state = actuator_hw.get_state(); +======= + state = actuator_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values state = actuator_hw.configure(); +<<<<<<< HEAD EXPECT_EQ(state_interfaces[0].get_value(), 0.0); EXPECT_EQ(command_interfaces[0].get_value(), 0.0); +======= + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -741,11 +1865,14 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) // Initiate error on write (this is the second time therefore unrecoverable) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); +<<<<<<< HEAD state = actuator_hw.get_state(); +======= + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -754,13 +1881,23 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// END -TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) +TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) { - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -781,14 +1918,17 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); - EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -804,7 +1944,8 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -814,12 +1955,22 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) { hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = sensor_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); + auto state = + sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -837,7 +1988,11 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); +<<<<<<< HEAD state = sensor_hw.get_state(); +======= + state = sensor_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -849,7 +2004,11 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); +<<<<<<< HEAD EXPECT_EQ(state_interfaces[0].get_value(), 0.0); +======= + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -862,7 +2021,11 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); +<<<<<<< HEAD state = sensor_hw.get_state(); +======= + state = sensor_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -877,13 +2040,86 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +<<<<<<< HEAD + +======= +// END + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_lifecycle_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_lifecycle_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST(TestComponentInterfaces, dummy_system_read_error_behavior) { hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = system_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -904,7 +2140,11 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); +<<<<<<< HEAD state = system_hw.get_state(); +======= + state = system_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -912,11 +2152,19 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { +<<<<<<< HEAD EXPECT_EQ(state_interfaces[index].get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { EXPECT_EQ(command_interfaces[index].get_value(), 0.0); +======= + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -932,7 +2180,84 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); +<<<<<<< HEAD state = system_hw.get_state(); +======= + state = system_hw.get_lifecycle_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + state = system_hw.get_lifecycle_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + state = system_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -942,12 +2267,22 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +<<<<<<< HEAD +======= +// BEGIN (Handle export change): for backward compatibility +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST(TestComponentInterfaces, dummy_system_write_error_behavior) { hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; +<<<<<<< HEAD auto state = system_hw.initialize(mock_hw_info); +======= + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -968,7 +2303,11 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); +<<<<<<< HEAD state = system_hw.get_state(); +======= + state = system_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -976,11 +2315,19 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { +<<<<<<< HEAD EXPECT_EQ(state_interfaces[index].get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { EXPECT_EQ(command_interfaces[index].get_value(), 0.0); +======= + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -996,7 +2343,86 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); +<<<<<<< HEAD state = system_hw.get_state(); +======= + state = system_hw.get_lifecycle_state(); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +<<<<<<< HEAD +======= +// END + +TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + state = system_hw.get_lifecycle_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -1005,3 +2431,11 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp new file mode 100644 index 0000000000..62e2b703cf --- /dev/null +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -0,0 +1,385 @@ +// Copyright 2020 ros2_control development team +// +// 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 +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +} // namespace + +using namespace ::testing; // NOLINT + +namespace test_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + std::string get_name() const override { return "DummyActuatorDefault"; } + + std::vector + export_unlisted_state_interface_descriptions() override + { + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + std::vector + export_unlisted_command_interface_descriptions() override + { + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + std::string get_name() const override { return "DummySensorDefault"; } + + std::vector + export_unlisted_state_interface_descriptions() override + { + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.sensors[0].name, info); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +class DummySystemDefault : public hardware_interface::SystemInterface +{ + std::string get_name() const override { return "DummySystemDefault"; } + + std::vector + export_unlisted_state_interface_descriptions() override + { + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + std::vector + export_unlisted_command_interface_descriptions() override + { + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_component"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(3u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(2u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_component"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + { + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); + ASSERT_TRUE(contains); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "sens1/some_unlisted_interface"); + ASSERT_TRUE(contains); + EXPECT_EQ("sens1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_system_default_custom_export) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_component"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(7u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(4u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index cdd2f2bb17..f0b78d821e 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -32,6 +32,10 @@ class TestComponentParser : public Test void SetUp() override {} }; +<<<<<<< HEAD +======= +using hardware_interface::HW_IF_ACCELERATION; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; @@ -112,8 +116,14 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly"); EXPECT_EQ(hardware_info.type, "system"); +<<<<<<< HEAD EXPECT_EQ( hardware_info.hardware_class_type, +======= + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ( + hardware_info.hardware_plugin_name, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -137,6 +147,33 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); +<<<<<<< HEAD +======= + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface) @@ -151,8 +188,14 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); EXPECT_EQ(hardware_info.type, "system"); +<<<<<<< HEAD EXPECT_EQ( hardware_info.hardware_class_type, +======= + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ( + hardware_info.hardware_plugin_name, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -174,6 +217,34 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); +<<<<<<< HEAD +======= + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) @@ -188,8 +259,14 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor"); EXPECT_EQ(hardware_info.type, "system"); +<<<<<<< HEAD EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); +======= + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -218,6 +295,135 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100"); +<<<<<<< HEAD +======= + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +} + +TEST_F( + TestComponentParser, + successfully_parse_valid_urdf_system_multi_interface_custom_interface_parameters) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_EQ(hardware_info.joints[0].parameters.size(), 3); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_ip"), "1.1.1.1"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_port"), "1234"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("use_persistent_connection"), "true"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3)); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); + // CommandInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].initial_value, "1.2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register"), "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].initial_value, "3.4"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].min, "-1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].max, "1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register"), "2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].min, "-0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].max, "0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].initial_value, ""); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].parameters.size(), 2); + + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register"), "3"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register"), "4"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].parameters.size(), 0); + + // Second Joint + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.joints[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_ip"), "192.168.178.123"); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_port"), "4321"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); + // CommandInterfaces + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].initial_value, ""); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.size(), 4); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("register"), "20"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("data_type"), "int32_t"); + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("data_type"), "int32_t"); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].parameters.size(), 0); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("data_type"), "int32_t"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) @@ -232,8 +438,14 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor"); EXPECT_EQ(hardware_info.type, "system"); +<<<<<<< HEAD EXPECT_EQ( hardware_info.hardware_class_type, +======= + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ( + hardware_info.hardware_plugin_name, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -248,6 +460,33 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte ASSERT_THAT(hardware_info.sensors, SizeIs(0)); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D"); @@ -259,6 +498,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); +<<<<<<< HEAD +======= + ASSERT_THAT(hardware_info.limits, SizeIs(0)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) @@ -272,28 +516,86 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot auto hardware_info = control_hardware.at(0); EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionActuatorHardware"); +======= + EXPECT_EQ(hardware_info.group, "Hardware Group"); + EXPECT_EQ(hardware_info.type, "actuator"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionActuatorHardware"); +======= + EXPECT_EQ(hardware_info.group, "Hardware Group"); + EXPECT_EQ(hardware_info.type, "actuator"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors) @@ -307,9 +609,16 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot auto hardware_info = control_hardware.at(0); EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware"); +======= + EXPECT_EQ(hardware_info.group, "Hardware Group 1"); + EXPECT_EQ(hardware_info.type, "actuator"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -328,13 +637,37 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info.transmissions[0].joints[0].mechanical_reduction, DoubleNear(1024.0 / M_PI, 0.01)); ASSERT_THAT(hardware_info.transmissions[0].actuators, SizeIs(1)); EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1"); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware"); +======= + EXPECT_EQ(hardware_info.group, "Hardware Group 2"); + EXPECT_EQ(hardware_info.type, "actuator"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); @@ -345,12 +678,44 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_info = control_hardware.at(2); EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint1"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.type, "sensor"); EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware"); +======= + EXPECT_EQ(hardware_info.group, "Hardware Group 1"); + EXPECT_EQ(hardware_info.type, "sensor"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -361,12 +726,36 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_info = control_hardware.at(3); EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint2"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.type, "sensor"); EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware"); +======= + EXPECT_EQ(hardware_info.group, "Hardware Group 2"); + EXPECT_EQ(hardware_info.type, "sensor"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -377,6 +766,30 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); +<<<<<<< HEAD +======= + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission) @@ -391,8 +804,14 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr EXPECT_EQ(hardware_info.name, "RRBotModularWrist"); EXPECT_EQ(hardware_info.type, "system"); +<<<<<<< HEAD EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); +======= + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -433,7 +852,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.name, "CameraWithIMU"); EXPECT_EQ(hardware_info.type, "sensor"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMUSensor"); +======= + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ(hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/CameraWithIMUSensor"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -449,6 +873,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image"); +<<<<<<< HEAD +======= + // There will be no limits as the ros2_control tag has only sensor info + ASSERT_THAT(hardware_info.limits, SizeIs(0)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) @@ -463,7 +893,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_EQ(hardware_info.name, "ActuatorModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( +<<<<<<< HEAD hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware"); +======= + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); @@ -498,6 +932,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_THAT(actuator.offset, DoubleEq(0.0)); ASSERT_THAT(transmission.parameters, SizeIs(1)); EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337"); +<<<<<<< HEAD +======= + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_locale_independent_double) @@ -537,7 +986,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( +<<<<<<< HEAD hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); +======= + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.joints, SizeIs(2)); @@ -562,9 +1015,39 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_THAT(hardware_info.gpios[1].state_interfaces, SizeIs(1)); EXPECT_THAT(hardware_info.gpios[1].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].name, "vacuum"); +<<<<<<< HEAD EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); EXPECT_THAT(hardware_info.transmissions, IsEmpty()); +======= + EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].initial_value, "1.0"); + EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); + + EXPECT_THAT(hardware_info.transmissions, IsEmpty()); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(1)); + char joint2_name[] = "joint2"; + for (const auto & joint : {"joint1", "joint2"}) + { + // Position limits are limited in the ros2_control tag + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + if (strcmp(joint, joint2_name) == 0) + { + EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5)); + } + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type) @@ -580,7 +1063,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.name, "RRBotSystemWithSizeAndDataType"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( +<<<<<<< HEAD hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType"); +======= + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType"); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_THAT(hardware_info.joints, SizeIs(1)); @@ -612,6 +1100,369 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); } +<<<<<<< HEAD +======= +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + + ASSERT_THAT(hardware_info.joints, SizeIs(3)); + + 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(5)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[2].name, "joint3"); + EXPECT_EQ(hardware_info.joints[2].type, "joint"); + EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2); + EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint2").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at("joint2").max_position, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at("joint2").min_position, DoubleNear(-1.5, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_position, DoubleNear(10.0, 1e-5)); + EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_velocity, DoubleNear(20.0, 1e-5)); + + EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable_interfaces) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + + ASSERT_THAT(hardware_info.joints, SizeIs(3)); + + 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(5)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[2].name, "joint3"); + EXPECT_EQ(hardware_info.joints[2].type, "joint"); + EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(0)); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint2").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)); + + EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); +} + +TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_revolute_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); + + urdf_to_test = std::string(ros2_control_test_assets::urdf_head_prismatic_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, throw_on_parse_urdf_system_with_command_fixed_joint) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::invalid_urdf_ros2_control_system_with_command_fixed_joint + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)) + << "velocity constraint from ros2_control_tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5)) + << "effort constraint from ros2_control_tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); +} + +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)) + << "velocity URDF constraint overridden by ros2_control tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)) + << "velocity constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -625,7 +1476,11 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Only"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( +<<<<<<< HEAD hardware_info.hardware_class_type, +======= + hardware_info.hardware_plugin_name, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); ASSERT_THAT(hardware_info.joints, SizeIs(1)); @@ -636,6 +1491,23 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), ""); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); +<<<<<<< HEAD +======= + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + ASSERT_THAT(hardware_info.soft_limits, SizeIs(0)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(TestComponentParser, negative_size_throws_error) @@ -671,3 +1543,277 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error) ros2_control_test_assets::urdf_tail; ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } +<<<<<<< HEAD +======= + +TEST_F(TestComponentParser, gripper_mimic_true_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +TEST_F(TestComponentParser, gripper_no_mimic_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +TEST_F(TestComponentParser, negative_rw_rates_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_with_negative_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_rw_rates_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_invalid_with_text_in_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_rw_rates_out_of_range) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_invalid_out_of_range_in_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_unknown_joint) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_true_without_mimic_info_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_no_mimic) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_true_invalid_config_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, gripper_mimic_false_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_false_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(0)); +} + +/** + * @brief Test that the parser throws an error if the URDF contains a link with no parent. + */ +TEST_F(TestComponentParser, urdf_two_root_links_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_invalid_two_root_links) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +/** + * @brief Test that the parser throws an error if a joint defined in the ros2_control tag is missing + * in the URDF + */ +TEST_F(TestComponentParser, urdf_incomplete_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_incomplete) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_command_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].sensors); + EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_command_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum"); + 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); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/test/test_components.hpp b/hardware_interface/test/test_components.hpp new file mode 100644 index 0000000000..def72b8398 --- /dev/null +++ b/hardware_interface/test/test_components.hpp @@ -0,0 +1,39 @@ +// Copyright 2020 ros2_control Development Team +// +// 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. + +#ifndef TEST_COMPONENTS_HPP_ +#define TEST_COMPONENTS_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" + +namespace test_components +{ + +template +std::pair vector_contains(const std::vector & vec, const T & element) +{ + auto it = std::find_if( + vec.begin(), vec.end(), [element](const auto & state_interface) + { return state_interface->get_name() == std::string(element); }); + + return std::make_pair(it != vec.end(), std::distance(vec.begin(), it)); +} + +} // namespace test_components +#endif // TEST_COMPONENTS_HPP_ diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..cabf783d59 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,16 @@ #include #include "hardware_interface/handle.hpp" +<<<<<<< HEAD using hardware_interface::CommandInterface; +======= +#include "hardware_interface/hardware_info.hpp" + +using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using hardware_interface::StateInterface; namespace @@ -64,3 +72,34 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } +<<<<<<< HEAD +======= + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index 47b19f9769..671e81a4bf 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -14,7 +14,10 @@ #include #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/sensor_interface.hpp" @@ -34,7 +37,11 @@ class TestForceTorqueSensor : public SensorInterface return CallbackReturn::ERROR; } +<<<<<<< HEAD const auto & state_interfaces = info_.sensors[0].state_interfaces; +======= + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (state_interfaces.size() != 6) { return CallbackReturn::ERROR; @@ -58,7 +65,11 @@ class TestForceTorqueSensor : public SensorInterface { std::vector state_interfaces; +<<<<<<< HEAD const auto & sensor_name = info_.sensors[0].name; +======= + const auto & sensor_name = get_hardware_info().sensors[0].name; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_hardware_components.xml b/hardware_interface/test/test_hardware_components/test_hardware_components.xml index b297bde668..4bd58a4e84 100644 --- a/hardware_interface/test/test_hardware_components/test_hardware_components.xml +++ b/hardware_interface/test/test_hardware_components/test_hardware_components.xml @@ -12,6 +12,15 @@ +<<<<<<< HEAD +======= + + + Test sensor component emulating an IMU sensor + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Test system component for a two degree of freedom position joint robot diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp new file mode 100644 index 0000000000..0bd9e9121c --- /dev/null +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -0,0 +1,141 @@ +// Copyright 2023 PAL Robotics SL. +// +// 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. + +/* + * Author: Sai Kishor Kothakota + */ + +#include +#include +#include +#include + +#include "hardware_interface/sensor_interface.hpp" + +using hardware_interface::return_type; +using hardware_interface::SensorInterface; +using hardware_interface::StateInterface; + +namespace test_hardware_components +{ +class TestIMUSensor : public SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & sensor_info) override + { + if (SensorInterface::on_init(sensor_info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; + if (state_interfaces.size() != 10) + { + return CallbackReturn::ERROR; + } + for (const auto & imu_key : + {"orientation.x", "orientation.y", "orientation.z", "orientation.w", "angular_velocity.x", + "angular_velocity.y", "angular_velocity.z", "linear_acceleration.x", + "linear_acceleration.y", "linear_acceleration.z"}) + { + if ( + std::find_if( + state_interfaces.begin(), state_interfaces.end(), [&imu_key](const auto & interface_info) + { return interface_info.name == imu_key; }) == state_interfaces.end()) + { + return CallbackReturn::ERROR; + } + } + + fprintf(stderr, "TestIMUSensor configured successfully.\n"); + return CallbackReturn::SUCCESS; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + + const std::string & sensor_name = get_hardware_info().sensors[0].name; + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.y", &orientation_.y)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.z", &orientation_.z)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "orientation.w", &orientation_.w)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "angular_velocity.x", &angular_velocity_.x)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "angular_velocity.y", &angular_velocity_.y)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor_name, "angular_velocity.z", &angular_velocity_.z)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor_name, "linear_acceleration.x", &linear_acceleration_.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor_name, "linear_acceleration.y", &linear_acceleration_.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor_name, "linear_acceleration.z", &linear_acceleration_.z)); + + return state_interfaces; + } + + return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + // generate a random distribution of the quaternion + std::uniform_real_distribution distribution_1(0.0, 1.0); + const double u1 = distribution_1(generator_); + const double u2 = distribution_1(generator_); + const double u3 = distribution_1(generator_); + orientation_.w = std::sqrt(1. - u1) * std::sin(2 * M_PI * u2); + orientation_.x = std::sqrt(1. - u1) * std::cos(2 * M_PI * u2); + orientation_.y = std::sqrt(u1) * std::sin(2 * M_PI * u3); + orientation_.z = std::sqrt(u1) * std::cos(2 * M_PI * u3); + + // generate random angular velocities and linear accelerations + std::uniform_real_distribution distribution_2(0.0, 0.1); + angular_velocity_.x = distribution_2(generator_); + angular_velocity_.y = distribution_2(generator_); + angular_velocity_.z = distribution_2(generator_); + + linear_acceleration_.x = distribution_2(generator_); + linear_acceleration_.y = distribution_2(generator_); + linear_acceleration_.z = distribution_2(generator_); + return return_type::OK; + } + +private: + struct QuaternionValues + { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double w = 1.0; + }; + struct AxisValues + { + double x = 0.0; + double y = 0.0; + double z = 0.0; + }; + + std::default_random_engine generator_; + QuaternionValues orientation_; + AxisValues angular_velocity_; + AxisValues linear_acceleration_; +}; + +} // namespace test_hardware_components + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(test_hardware_components::TestIMUSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 390056d990..e43a463b22 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/actuator_interface.hpp" @@ -35,12 +38,20 @@ class TestSingleJointActuator : public ActuatorInterface } // can only control one joint +<<<<<<< HEAD if (info_.joints.size() != 1) +======= + if (get_hardware_info().joints.size() != 1) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return CallbackReturn::ERROR; } // can only control in position +<<<<<<< HEAD const auto & command_interfaces = info_.joints[0].command_interfaces; +======= + const auto & command_interfaces = get_hardware_info().joints[0].command_interfaces; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (command_interfaces.size() != 1) { return CallbackReturn::ERROR; @@ -50,7 +61,11 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } // can only give feedback state for position and velocity +<<<<<<< HEAD const auto & state_interfaces = info_.joints[0].state_interfaces; +======= + const auto & state_interfaces = get_hardware_info().joints[0].state_interfaces; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if (state_interfaces.size() < 1) { return CallbackReturn::ERROR; @@ -72,7 +87,11 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector state_interfaces; +<<<<<<< HEAD const auto & joint_name = info_.joints[0].name; +======= + const auto & joint_name = get_hardware_info().joints[0].name; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -85,7 +104,11 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector command_interfaces; +<<<<<<< HEAD const auto & joint_name = info_.joints[0].name; +======= + const auto & joint_name = get_hardware_info().joints[0].name; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index a594d3b70a..5eb56385ac 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -32,11 +32,19 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface } // Can only control two joints +<<<<<<< HEAD if (info_.joints.size() != 2) { return CallbackReturn::ERROR; } for (const hardware_interface::ComponentInfo & joint : info_.joints) +======= + if (get_hardware_info().joints.size() != 2) + { + return CallbackReturn::ERROR; + } + for (const hardware_interface::ComponentInfo & joint : get_hardware_info().joints) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // Can control in position or velocity const auto & command_interfaces = joint.command_interfaces; @@ -77,6 +85,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -85,6 +94,19 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION, + &acceleration_state_[i])); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return state_interfaces; @@ -93,12 +115,23 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_command_[i])); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return command_interfaces; @@ -127,6 +160,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface stop_modes_.clear(); for (const auto & key : start_interfaces) { +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) @@ -134,13 +168,26 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface start_modes_.push_back(hardware_interface::HW_IF_POSITION); } if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) + { + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_POSITION) + { + start_modes_.push_back(hardware_interface::HW_IF_POSITION); + } + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } } } // Example Criteria 1 - Starting: All interfaces must be given a new mode at the same time +<<<<<<< HEAD if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) +======= + if (start_modes_.size() != 0 && start_modes_.size() != get_hardware_info().joints.size()) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return hardware_interface::return_type::ERROR; } @@ -148,9 +195,15 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface // Stopping interfaces for (const auto & key : stop_interfaces) { +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); i++) { if (key.find(info_.joints[i].name) != std::string::npos) +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) + { + if (key.find(get_hardware_info().joints[i].name) != std::string::npos) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { stop_modes_.push_back(true); } @@ -184,11 +237,19 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector start_modes_ = {"position", "position"}; std::vector stop_modes_ = {false, false}; +<<<<<<< HEAD std::array position_command_ = {0.0, 0.0}; std::array velocity_command_ = {0.0, 0.0}; std::array position_state_ = {0.0, 0.0}; std::array velocity_state_ = {0.0, 0.0}; std::array acceleration_state_ = {0.0, 0.0}; +======= + std::array position_command_ = {{0.0, 0.0}}; + std::array velocity_command_ = {{0.0, 0.0}}; + std::array position_state_ = {{0.0, 0.0}}; + std::array velocity_state_ = {{0.0, 0.0}}; + std::array acceleration_state_ = {{0.0, 0.0}}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index b597804104..9766216bf0 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -13,7 +13,10 @@ // limitations under the License. #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/system_interface.hpp" @@ -36,11 +39,19 @@ class TestTwoJointSystem : public SystemInterface } // can only control two joint +<<<<<<< HEAD if (info_.joints.size() != 2) { return CallbackReturn::ERROR; } for (const auto & joint : info_.joints) +======= + if (get_hardware_info().joints.size() != 2) + { + return CallbackReturn::ERROR; + } + for (const auto & joint : get_hardware_info().joints) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { // can only control in position const auto & command_interfaces = joint.command_interfaces; @@ -71,10 +82,18 @@ class TestTwoJointSystem : public SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return state_interfaces; @@ -83,10 +102,18 @@ class TestTwoJointSystem : public SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; +<<<<<<< HEAD for (auto i = 0u; i < info_.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); +======= + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return command_interfaces; @@ -103,8 +130,13 @@ class TestTwoJointSystem : public SystemInterface } private: +<<<<<<< HEAD std::array position_command_ = {0.0, 0.0}; std::array position_state_ = {0.0, 0.0}; +======= + std::array position_command_ = {{0.0, 0.0}}; + std::array position_state_ = {{0.0, 0.0}}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_inst_hardwares.cpp b/hardware_interface/test/test_inst_hardwares.cpp index ddd3aea0ad..a3868a90f3 100644 --- a/hardware_interface/test/test_inst_hardwares.cpp +++ b/hardware_interface/test/test_inst_hardwares.cpp @@ -15,11 +15,16 @@ #include #include "hardware_interface/actuator.hpp" +<<<<<<< HEAD #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" +======= +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class TestInstantiationHardwares : public ::testing::Test { diff --git a/hardware_interface/test/test_macros.cpp b/hardware_interface/test/test_macros.cpp index 7dc707d58b..e7a8923124 100644 --- a/hardware_interface/test/test_macros.cpp +++ b/hardware_interface/test/test_macros.cpp @@ -12,12 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +<<<<<<< HEAD +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/macros.hpp" +<<<<<<< HEAD #include "gmock/gmock.h" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class TestMacros : public ::testing::Test { protected: diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index eccffe654f..6328c91c04 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -230,3 +231,81 @@ Changelog for package hardware_interface_testing 0.1.0 (2020-12-22) ------------------ +======= +4.21.0 (2024-12-06) +------------------- +* [Feature] Choose different read and write rate for the hardware components (`#1570 `_) +* Contributors: Sai Kishor Kothakota + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- + +4.18.0 (2024-10-07) +------------------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* Contributors: Manuel Muth + +4.17.0 (2024-09-11) +------------------- + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* Contributors: Sai Kishor Kothakota + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Fix typos in test_resource_manager.cpp (`#1609 `_) +* Contributors: Henry Moore, Parker Drouillard + +4.13.0 (2024-07-08) +------------------- +* [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) +* Contributors: Sai Kishor Kothakota + +4.12.0 (2024-07-01) +------------------- +* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 `_) +* Contributors: Dr. Denis + +4.11.0 (2024-05-14) +------------------- + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + +4.8.0 (2024-03-27) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-03-02) +------------------ +* Add -Werror=missing-braces to compile options (`#1423 `_) +* Contributors: Sai Kishor Kothakota + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ +* Fix version +* Move `test_components` to own package (`#1325 `_) +* Contributors: Bence Magyar, Christoph Fröhlich +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index ec6d591dee..2e4b80cf51 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -2,7 +2,12 @@ cmake_minimum_required(VERSION 3.16) project(hardware_interface_testing LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") +<<<<<<< HEAD add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +======= + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Werror=missing-braces) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -20,6 +25,7 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(test_components SHARED +<<<<<<< HEAD test/test_components/test_actuator.cpp test/test_components/test_sensor.cpp test/test_components/test_system.cpp @@ -27,11 +33,23 @@ add_library(test_components SHARED ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets) install(TARGETS test_components DESTINATION lib +======= +test/test_components/test_actuator.cpp +test/test_components/test_sensor.cpp +test/test_components/test_system.cpp) +ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets) +install(TARGETS test_components +DESTINATION lib +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) pluginlib_export_plugin_description_file( hardware_interface test/test_components/test_components.xml) if(BUILD_TESTING) +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) find_package(ament_cmake_gmock REQUIRED) ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 424682ca91..ece985006a 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,11 @@ hardware_interface_testing +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 5d532919e1..25083fe7f9 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -12,10 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. +<<<<<<< HEAD #include #include #include "hardware_interface/actuator_interface.hpp" +======= +#include + +#include "hardware_interface/actuator_interface.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using hardware_interface::ActuatorInterface; using hardware_interface::CommandInterface; @@ -31,15 +38,33 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } +<<<<<<< HEAD +======= + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") + { + return CallbackReturn::ERROR; + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /* * a hardware can optional prove for incorrect info here. * * // can only control one joint +<<<<<<< HEAD * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position * if (info_.joints[0].command_interfaces.size() != 1) {return CallbackReturn::ERROR;} * // can only give feedback state for position and velocity * if (info_.joints[0].state_interfaces.size() != 2) {return CallbackReturn::ERROR;} +======= + * if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;} + * // can only control in position + * if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return + * CallbackReturn::ERROR;} + * // can only give feedback state for position and velocity + * if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return + * CallbackReturn::ERROR;} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) */ return CallbackReturn::SUCCESS; @@ -49,11 +74,21 @@ class TestActuator : public ActuatorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( +<<<<<<< HEAD info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); +======= + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name, + &position_state_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name, + &velocity_state_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return state_interfaces; } @@ -62,12 +97,23 @@ class TestActuator : public ActuatorInterface { std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( +<<<<<<< HEAD info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); if (info_.joints[0].command_interfaces.size() > 1) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); +======= + get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name, + &velocity_command_)); + + if (get_hardware_info().joints[0].command_interfaces.size() > 1) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + get_hardware_info().joints[0].name, + get_hardware_info().joints[0].command_interfaces[1].name, &max_velocity_command_)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return command_interfaces; @@ -92,6 +138,7 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read +<<<<<<< HEAD if (velocity_command_ == 28282828.0) { // reset value to get out from error on the next call - simplifies CM tests @@ -102,6 +149,25 @@ class TestActuator : public ActuatorInterface // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some // other mixture of interfaces somewhere in the test stack. +======= + if (velocity_command_ == test_constants::READ_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM + // tests + velocity_command_ = 0.0; + return return_type::ERROR; + } + // simulate deactivate on read + if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } + // The next line is for the testing purposes. We need value to be changed to + // be sure that the feedback from hardware to controllers in the chain is + // working as it should. This makes value checks clearer and confirms there + // is no "state = command" line or some other mixture of interfaces + // somewhere in the test stack. +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) velocity_state_ = velocity_command_ / 2; return return_type::OK; } @@ -109,12 +175,27 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write +<<<<<<< HEAD if (velocity_command_ == 23232323.0) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } +======= + if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM + // tests + velocity_command_ = 0.0; + return return_type::ERROR; + } + // simulate deactivate on write + if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } @@ -125,7 +206,11 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; +<<<<<<< HEAD class TestUnitilizableActuator : public TestActuator +======= +class TestUninitializableActuator : public TestActuator +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -136,4 +221,8 @@ class TestUnitilizableActuator : public TestActuator #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) +<<<<<<< HEAD PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) +======= +PLUGINLIB_EXPORT_CLASS(TestUninitializableActuator, hardware_interface::ActuatorInterface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index f029d3dd37..4ef4f12a23 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -18,6 +18,7 @@ +<<<<<<< HEAD Test Unitilizable Actuator @@ -33,6 +34,23 @@ Test Unitilizable System +======= + + + Test Uninitializable Actuator + + + + + + Test Uninitializable Sensor + + + + + + Test Uninitializable System +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 4811244138..3766775852 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/sensor_interface.hpp" @@ -30,7 +33,11 @@ class TestSensor : public SensorInterface return CallbackReturn::ERROR; } // can only give feedback state for velocity +<<<<<<< HEAD if (info_.sensors[0].state_interfaces.size() != 1) +======= + if (get_hardware_info().sensors[0].state_interfaces.size() == 2) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { return CallbackReturn::ERROR; } @@ -41,7 +48,12 @@ class TestSensor : public SensorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( +<<<<<<< HEAD info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); +======= + get_hardware_info().sensors[0].name, get_hardware_info().sensors[0].state_interfaces[0].name, + &velocity_state_)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return state_interfaces; } @@ -55,7 +67,11 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; +<<<<<<< HEAD class TestUnitilizableSensor : public TestSensor +======= +class TestUninitializableSensor : public TestSensor +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -66,4 +82,8 @@ class TestUnitilizableSensor : public TestSensor #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) +<<<<<<< HEAD PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) +======= +PLUGINLIB_EXPORT_CLASS(TestUninitializableSensor, hardware_interface::SensorInterface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 9792286796..bf0043ce32 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -13,13 +13,20 @@ // limitations under the License. #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +<<<<<<< HEAD #include "rcutils/logging_macros.h" +======= +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using hardware_interface::CommandInterface; using hardware_interface::return_type; @@ -28,6 +35,7 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { +<<<<<<< HEAD std::vector export_state_interfaces() override { std::vector state_interfaces; @@ -49,6 +57,43 @@ class TestSystem : public SystemInterface // Add configuration/max_tcp_jerk interface state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[2].name, info_.joints[2].state_interfaces[0].name, &configuration_state_)); +======= + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // Simulating initialization error + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") + { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + + std::vector export_state_interfaces() override + { + const auto info = get_hardware_info(); + std::vector state_interfaces; + for (auto i = 0u; i < info.joints.size(); ++i) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + } + + if (info.gpios.size() > 0) + { + // Add configuration/max_tcp_jerk interface + state_interfaces.emplace_back(hardware_interface::StateInterface( + info.gpios[0].name, info.gpios[0].state_interfaces[0].name, &configuration_state_)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return state_interfaces; @@ -56,6 +101,7 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { +<<<<<<< HEAD RCUTILS_LOG_INFO_NAMED("test_system", "Exporting configuration interfaces."); std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) @@ -76,6 +122,24 @@ class TestSystem : public SystemInterface // Add configuration/max_tcp_jerk interface command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[2].name, info_.joints[2].command_interfaces[0].name, &configuration_command_)); +======= + const auto info = get_hardware_info(); + std::vector command_interfaces; + for (auto i = 0u; i < info.joints.size(); ++i) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + } + // Add max_acceleration command interface + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info.joints[0].name, info.joints[0].command_interfaces[1].name, &max_acceleration_command_)); + + if (info.gpios.size() > 0) + { + // Add configuration/max_tcp_jerk interface + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info.gpios[0].name, info.gpios[0].command_interfaces[0].name, &configuration_command_)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } return command_interfaces; @@ -84,38 +148,85 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read +<<<<<<< HEAD if (velocity_command_[0] == 28282828) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } +======= + if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM + // tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } + // simulate deactivate on read + if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } + // The next line is for the testing purposes. We need value to be changed to + // be sure that the feedback from hardware to controllers in the chain is + // working as it should. This makes value checks clearer and confirms there + // is no "state = command" line or some other mixture of interfaces + // somewhere in the test stack. + velocity_state_[0] = velocity_command_[0] / 2.0; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write +<<<<<<< HEAD if (velocity_command_[0] == 23232323) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } +======= + if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + { + // reset value to get out from error on the next call - simplifies CM + // tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } + // simulate deactivate on write + if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return return_type::OK; } private: +<<<<<<< HEAD std::array velocity_command_ = {0.0, 0.0}; std::array position_state_ = {0.0, 0.0}; std::array velocity_state_ = {0.0, 0.0}; std::array acceleration_state_ = {0.0, 0.0}; +======= + std::array velocity_command_ = {{0.0, 0.0}}; + std::array position_state_ = {{0.0, 0.0}}; + std::array velocity_state_ = {{0.0, 0.0}}; + std::array acceleration_state_ = {{0.0, 0.0}}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) double max_acceleration_command_ = 0.0; double configuration_state_ = 0.0; double configuration_command_ = 0.0; }; +<<<<<<< HEAD class TestUnitilizableSystem : public TestSystem +======= +class TestUninitializableSystem : public TestSystem +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -126,4 +237,8 @@ class TestUnitilizableSystem : public TestSystem #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) +<<<<<<< HEAD PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) +======= +PLUGINLIB_EXPORT_CLASS(TestUninitializableSystem, hardware_interface::SystemInterface) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 1157aa74e3..4a0ac30ef8 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -27,6 +27,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" +<<<<<<< HEAD using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; @@ -43,6 +44,26 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; +======= +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" + +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_PLUGIN_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_PLUGIN_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; +using testing::SizeIs; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto configure_components = [](TestableResourceManager & rm, const std::vector & components = {}) @@ -86,16 +107,25 @@ auto shutdown_components = TEST_F(ResourceManagerTest, initialization_empty) { +<<<<<<< HEAD ASSERT_ANY_THROW(TestableResourceManager rm("")); +======= + ASSERT_ANY_THROW(TestableResourceManager rm(node_, "");); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ResourceManagerTest, initialization_with_urdf) { +<<<<<<< HEAD ASSERT_NO_THROW(TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); +======= + ASSERT_NO_THROW(TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf);); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ResourceManagerTest, post_initialization_with_urdf) { +<<<<<<< HEAD TestableResourceManager rm; ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } @@ -116,6 +146,24 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) // the interface should not show up TestableResourceManager rm; EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); +======= + TestableResourceManager rm(node_); + ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); +} + +void test_load_and_initialized_components_failure(const std::string & urdf) +{ + rclcpp::Node node = rclcpp::Node("TestableResourceManager"); + TestableResourceManager rm(node); + ASSERT_FALSE(rm.load_and_initialize_components(urdf)); + + ASSERT_FALSE(rm.are_components_initialized()); + + // resource manager should also not have any components + EXPECT_EQ(rm.actuator_components_size(), 0); + EXPECT_EQ(rm.sensor_components_size(), 0); + EXPECT_EQ(rm.system_components_size(), 0); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -139,17 +187,37 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); } +<<<<<<< HEAD TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); +======= +TEST_F(ResourceManagerTest, test_uninitializable_hardware) +{ + SCOPED_TRACE("test_uninitializable_hardware_no_validation"); + // If the the hardware can not be initialized and load_and_initialize_components didn't try to + // validate the interfaces, the interface should not show up + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_uninitializable_robot_urdf); +} + +TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) +{ + // we validate the results manually + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf, false); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.sensor_components_size()); EXPECT_EQ(1u, rm.system_components_size()); auto state_interface_keys = rm.state_interface_keys(); +<<<<<<< HEAD ASSERT_EQ(11u, state_interface_keys.size()); +======= + ASSERT_THAT(state_interface_keys, SizeIs(11)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); @@ -157,12 +225,17 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.state_interface_exists("joint3/position")); auto command_interface_keys = rm.command_interface_keys(); +<<<<<<< HEAD ASSERT_EQ(6u, command_interface_keys.size()); +======= + ASSERT_THAT(command_interface_keys, SizeIs(6)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } +<<<<<<< HEAD TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // missing state keys @@ -177,12 +250,29 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), std::exception); } +======= +TEST_F(ResourceManagerTest, expect_validation_failure_if_not_all_interfaces_are_exported) +{ + SCOPED_TRACE("missing state keys"); + // missing state keys + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf); + + SCOPED_TRACE("missing command keys"); + // missing command keys + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) { // we validate the results manually +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto command_interface_keys = rm.command_interface_keys(); for (const auto & key : command_interface_keys) @@ -198,6 +288,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +<<<<<<< HEAD TEST_F(ResourceManagerTest, no_load_urdf_function_called) { TestableResourceManager rm; @@ -224,11 +315,86 @@ TEST_F(ResourceManagerTest, can_load_urdf_later) ASSERT_FALSE(rm.is_urdf_already_loaded()); rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); +======= +TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) +{ + TestableResourceManager rm(node_); + ASSERT_FALSE(rm.are_components_initialized()); +} + +TEST_F( + ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_plugin_does_not_exist) +{ + SCOPED_TRACE("Actuator plugin does not exist"); + // Actuator + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_actuator_plugin); + + SCOPED_TRACE("Sensor plugin does not exist"); + // Sensor + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_sensors_plugin); + + SCOPED_TRACE("System plugin does not exist"); + // System + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_system_plugin); +} + +TEST_F(ResourceManagerTest, expect_load_and_initialize_to_fail_when_there_are_dupplicate_of_hw_comp) +{ + SCOPED_TRACE("Duplicated components"); + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_duplicated_component); +} + +TEST_F( + ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_initialization_fails) +{ + SCOPED_TRACE("Actuator initialization fails"); + // Actuator + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_actuator_initialization_error); + + SCOPED_TRACE("Sensor initialization fails"); + // Sensor + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_sensor_initialization_error); + + SCOPED_TRACE("System initialization fails"); + // System + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_system_initialization_error); +} + +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) +{ + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.are_components_initialized()); +} + +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_async_urdf_is_valid) +{ + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_async_robot_urdf); + ASSERT_TRUE(rm.are_components_initialized()); +} + +TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) +{ + TestableResourceManager rm(node_); + ASSERT_FALSE(rm.are_components_initialized()); + rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.are_components_initialized()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ResourceManagerTest, resource_claiming) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Activate components to get all interfaces available activate_components(rm); @@ -251,11 +417,16 @@ TEST_F(ResourceManagerTest, resource_claiming) } // command interfaces can only be claimed once +<<<<<<< HEAD for (const auto & interface_key : +======= + for (const auto & key : +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) {"joint1/position", "joint1/position", "joint1/position", "joint2/velocity", "joint3/velocity"}) { { +<<<<<<< HEAD auto interface = rm.claim_command_interface(interface_key); EXPECT_TRUE(rm.command_interface_is_available(interface_key)); EXPECT_TRUE(rm.command_interface_is_claimed(interface_key)); @@ -266,26 +437,51 @@ TEST_F(ResourceManagerTest, resource_claiming) } EXPECT_TRUE(rm.command_interface_is_available(interface_key)); EXPECT_FALSE(rm.command_interface_is_claimed(interface_key)); +======= + auto interface = rm.claim_command_interface(key); + EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); + EXPECT_TRUE(rm.command_interface_is_available(key)); + } + } + EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_FALSE(rm.command_interface_is_claimed(key)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // TODO(destogl): This claim test is not true.... can not be... // state interfaces can be claimed multiple times +<<<<<<< HEAD for (const auto & interface_key : +======= + for (const auto & key : +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) {"joint1/position", "joint1/velocity", "sensor1/velocity", "joint2/position", "joint3/position"}) { { +<<<<<<< HEAD EXPECT_TRUE(rm.state_interface_is_available(interface_key)); auto interface = rm.claim_state_interface(interface_key); { EXPECT_TRUE(rm.state_interface_is_available(interface_key)); EXPECT_NO_THROW(rm.claim_state_interface(interface_key)); +======= + EXPECT_TRUE(rm.state_interface_is_available(key)); + auto interface = rm.claim_state_interface(key); + { + EXPECT_TRUE(rm.state_interface_is_available(key)); + EXPECT_NO_THROW(rm.claim_state_interface(key)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } } std::vector interfaces; const auto interface_names = {"joint1/position", "joint2/velocity", "joint3/velocity"}; +<<<<<<< HEAD for (const auto & interface : interface_names) { EXPECT_TRUE(rm.command_interface_is_available(interface)); @@ -301,6 +497,23 @@ TEST_F(ResourceManagerTest, resource_claiming) { EXPECT_TRUE(rm.command_interface_is_available(interface)); EXPECT_FALSE(rm.command_interface_is_claimed(interface)); +======= + for (const auto & key : interface_names) + { + EXPECT_TRUE(rm.command_interface_is_available(key)); + interfaces.emplace_back(rm.claim_command_interface(key)); + } + for (const auto & key : interface_names) + { + EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + } + interfaces.clear(); + for (const auto & key : interface_names) + { + EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_FALSE(rm.command_interface_is_claimed(key)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } } @@ -342,7 +555,11 @@ class ExternalComponent : public hardware_interface::ActuatorInterface TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Activate components to get all interfaces available activate_components(rm); @@ -350,18 +567,33 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) EXPECT_EQ(1u, rm.sensor_components_size()); EXPECT_EQ(1u, rm.system_components_size()); +<<<<<<< HEAD ASSERT_EQ(11u, rm.state_interface_keys().size()); ASSERT_EQ(6u, rm.command_interface_keys().size()); +======= + ASSERT_THAT(rm.state_interface_keys(), SizeIs(11)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(6)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; external_component_hw_info.type = "actuator"; +<<<<<<< HEAD rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); ASSERT_EQ(12u, rm.state_interface_keys().size()); EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); ASSERT_EQ(7u, rm.command_interface_keys().size()); +======= + external_component_hw_info.is_async = false; + rm.import_component(std::make_unique(), external_component_hw_info); + EXPECT_EQ(2u, rm.actuator_components_size()); + + ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); + EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(rm.command_interface_exists("external_joint/external_command_interface")); auto status_map = rm.get_components_status(); @@ -385,7 +617,11 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) TEST_F(ResourceManagerTest, default_prepare_perform_switch) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Activate components to get all interfaces available activate_components(rm); @@ -396,7 +632,12 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) TEST_F(ResourceManagerTest, resource_status) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm( + node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto status_map = rm.get_components_status(); @@ -408,10 +649,22 @@ TEST_F(ResourceManagerTest, resource_status) EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE); +<<<<<<< HEAD // class_type EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].class_type, TEST_ACTUATOR_HARDWARE_CLASS_TYPE); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].class_type, TEST_SENSOR_HARDWARE_CLASS_TYPE); EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].class_type, TEST_SYSTEM_HARDWARE_CLASS_TYPE); +======= + // read/write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].rw_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate, 25u); + // plugin_name + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].plugin_name, TEST_ACTUATOR_HARDWARE_PLUGIN_NAME); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].plugin_name, TEST_SENSOR_HARDWARE_PLUGIN_NAME); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].plugin_name, TEST_SYSTEM_HARDWARE_PLUGIN_NAME); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // state EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -468,7 +721,11 @@ TEST_F(ResourceManagerTest, resource_status) TEST_F(ResourceManagerTest, lifecycle_all_resources) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // All resources start as UNCONFIGURED { @@ -611,7 +868,11 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) TEST_F(ResourceManagerTest, lifecycle_individual_resources) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // All resources start as UNCONFIGURED { @@ -824,7 +1085,11 @@ TEST_F(ResourceManagerTest, lifecycle_individual_resources) TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { using std::placeholders::_1; +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto check_interfaces = [](const std::vector & interface_names, auto check_method, bool expected_result) @@ -871,7 +1136,12 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) std::bind(&TestableResourceManager::command_interface_is_claimed, &rm, _1), expected_result); }; +<<<<<<< HEAD // All resources start as UNCONFIGURED - All interfaces are imported but not available +======= + // All resources start as UNCONFIGURED - All interfaces are imported but not + // available +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, @@ -951,7 +1221,12 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, false); } +<<<<<<< HEAD // When actuator is activated all state- and command- interfaces become available +======= + // When actuator is activated all state- and command- interfaces become + // available +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) activate_components(rm, {TEST_ACTUATOR_HARDWARE_NAME}); { check_interfaces( @@ -1170,7 +1445,11 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::string CONTROLLER_NAME = "test_controller"; std::vector REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"}; @@ -1179,12 +1458,20 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; +<<<<<<< HEAD std::vector reference_interfaces; +======= + std::vector reference_interfaces; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { +<<<<<<< HEAD reference_interfaces.push_back(hardware_interface::CommandInterface( +======= + reference_interfaces.push_back(std::make_shared( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); } @@ -1285,14 +1572,21 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) } class ResourceManagerTestReadWriteError : public ResourceManagerTest +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { public: void setup_resource_manager_and_do_initial_checks() { rm = std::make_shared( +<<<<<<< HEAD ros2_control_test_assets::minimal_robot_urdf, false); +======= + node_, ros2_control_test_assets::minimal_robot_urdf, false); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) activate_components(*rm); auto status_map = rm->get_components_status(); @@ -1429,7 +1723,12 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest check_if_interface_available(true, true); } +<<<<<<< HEAD // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME +======= + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and + // TEST_SYSTEM_HARDWARE_NAME +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) claimed_itfs[0].set_value(fail_value); claimed_itfs[1].set_value(fail_value); { @@ -1467,6 +1766,126 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } } +<<<<<<< HEAD +======= + void check_read_or_write_deactivate( + FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // deactivate for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value - 10.0); + { + // deactivate on error + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + 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_ACTIVE); + check_if_interface_available(true, true); + + // reactivate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + 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); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value - 10.0); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + 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_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + 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); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and + // TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + 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); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + 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); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) public: std::shared_ptr rm; std::vector claimed_itfs; @@ -1475,8 +1894,11 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); // values to set to hardware to simulate failure on read and write +<<<<<<< HEAD static constexpr double READ_FAIL_VALUE = 28282828.0; static constexpr double WRITE_FAIL_VALUE = 23232323.0; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) @@ -1487,7 +1909,11 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) // check read methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::read, rm, _1, _2), +<<<<<<< HEAD std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); +======= + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) @@ -1498,12 +1924,42 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) // check write methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::write, rm, _1, _2), +<<<<<<< HEAD std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); +======= + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) { +<<<<<<< HEAD TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); +======= + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf, false); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) activate_components(rm); static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator"; @@ -1545,3 +2001,233 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); } } +<<<<<<< HEAD +======= + +class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate, false); + activate_components(*rm); + + cm_update_rate_ = 100u; // The default value inside + time = node_.get_clock()->now(); + + 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); + + // read/write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].rw_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate, 25u); + + actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate; + system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate; + + 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); + } + }; + + using FunctionT = + std::function; + + void check_read_and_write_cycles(bool test_for_changing_values) + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + + 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()); + if (i % (cm_update_rate_ / system_rw_rate_) == 0 && test_for_changing_values) + { + // The values are computations exactly within the test_components + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + 20.0); + } + if (i % (cm_update_rate_ / actuator_rw_rate_) == 0 && test_for_changing_values) + { + // The values are computations exactly within the test_components + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); + } + // Even though we skip some read and write iterations, the state interfaces should be the same + // as previous updated one until the next cycle + ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value); + ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + } + +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); + + // values to set to hardware to simulate failure on read and write +}; + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_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( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_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( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_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); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 46a487f2ef..962ca2240f 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -22,6 +22,11 @@ #include #include +<<<<<<< HEAD +======= +#include + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -31,6 +36,11 @@ class ResourceManagerTest : public ::testing::Test static void SetUpTestCase() {} void SetUp() {} +<<<<<<< HEAD +======= + + rclcpp::Node node_{"ResourceManagerTest"}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; // Forward declaration @@ -44,6 +54,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager public: friend ResourceManagerTest; +<<<<<<< HEAD FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); @@ -55,6 +66,24 @@ class TestableResourceManager : public hardware_interface::ResourceManager TestableResourceManager( const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) +======= + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_and_manual_validation); + FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); + FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); + FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_uninitializable_hardware_no_validation); + + explicit TestableResourceManager(rclcpp::Node & node) + : hardware_interface::ResourceManager( + node.get_node_clock_interface(), node.get_node_logging_interface()) + { + } + + explicit TestableResourceManager( + rclcpp::Node & node, const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager( + urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { } }; diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index 0e93e990e6..0d2599f29f 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -20,7 +20,10 @@ #include #include "hardware_interface/loaned_state_interface.hpp" +<<<<<<< HEAD #include "lifecycle_msgs/msg/state.hpp" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "ros2_control_test_assets/descriptions.hpp" const auto hardware_resources_command_modes = @@ -65,7 +68,11 @@ class ResourceManagerPreparePerformTest : public ResourceManagerTest { ResourceManagerTest::SetUp(); +<<<<<<< HEAD rm_ = std::make_unique(command_mode_urdf); +======= + rm_ = std::make_unique(node_, command_mode_urdf); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ASSERT_EQ(1u, rm_->actuator_components_size()); ASSERT_EQ(1u, rm_->system_components_size()); @@ -105,6 +112,10 @@ class ResourceManagerPreparePerformTest : public ResourceManagerTest std::unique_ptr claimed_actuator_position_state_; // Scenarios defined by example criteria +<<<<<<< HEAD +======= + rclcpp::Node node_{"ResourceManagerPreparePerformTest"}; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) std::vector empty_keys = {}; std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; std::vector legal_keys_system = {"joint1/position", "joint2/position"}; @@ -367,24 +378,46 @@ TEST_F( EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); // When TestActuatorHardware is INACTIVE expect OK +<<<<<<< HEAD EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); +======= + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); +<<<<<<< HEAD EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); +======= + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); +<<<<<<< HEAD EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); +======= + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); }; +<<<<<<< HEAD +======= + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 52418676ba..fff9f6f127 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -127,6 +128,188 @@ Changelog for package joint_limits 2.16.0 (2022-10-17) ------------------- +======= +4.21.0 (2024-12-06) +------------------- +* Use the .hpp headers from realtime_tools package (`#1916 `_) +* Contributors: Christoph Fröhlich + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- + +4.18.0 (2024-10-07) +------------------- + +4.17.0 (2024-09-11) +------------------- + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + +4.13.0 (2024-07-08) +------------------- +* [JointLimits] Add Saturation Joint Limiter that uses clamping method (`#971 `_) +* Contributors: Dr. Denis + +4.12.0 (2024-07-01) +------------------- +* Reactivate generate_version_header (`#1544 `_) +* Bump version of pre-commit hooks (`#1556 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + +4.11.0 (2024-05-14) +------------------- +* Fix dependencies for source build (`#1533 `_) +* Add find_package for ament_cmake_gen_version_h (`#1534 `_) +* Contributors: Christoph Fröhlich + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ + +4.8.0 (2024-03-27) +------------------ +* generate version.h file per package using the ament_generate_version_header (`#1449 `_) +* Contributors: Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* Codeformat from new pre-commit config (`#1433 `_) +* Contributors: Christoph Fröhlich + +4.6.0 (2024-03-02) +------------------ +* Add -Werror=missing-braces to compile options (`#1423 `_) +* Contributors: Sai Kishor Kothakota + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ +* [Format] Correct formatting of JointLimits URDF file. (`#1339 `_) +* Add header to import limits from standard URDF definition (`#1298 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.3.0 (2024-01-20) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- +* Add -Wconversion flag to protect future developments (`#1053 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#1037 `_) +* Contributors: Sai Kishor Kothakota, gwalck + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- +* Extend joint limits structure with deceleration limits. (`#977 `_) +* Contributors: Dr. Denis + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Contributors: Dr. Denis, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 9356a96ea4..1e2e990ee2 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(joint_limits) @@ -16,6 +17,31 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +======= +cmake_minimum_required(VERSION 3.16) +project(joint_limits LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Werror=missing-braces) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib + realtime_tools + rclcpp + rclcpp_lifecycle + trajectory_msgs + urdf +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(ament_cmake_gen_version_h REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) add_library(joint_limits INTERFACE) target_compile_features(joint_limits INTERFACE cxx_std_17) @@ -25,6 +51,7 @@ target_include_directories(joint_limits INTERFACE ) ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) +<<<<<<< HEAD install(DIRECTORY include/ DESTINATION include ) @@ -49,6 +76,70 @@ if(BUILD_TESTING) FILES test/joint_limits_rosparam.yaml DESTINATION share/${PROJECT_NAME}/test +======= +add_library(joint_limiter_interface SHARED + src/joint_limiter_interface.cpp +) +target_compile_features(joint_limiter_interface PUBLIC cxx_std_17) +target_include_directories(joint_limiter_interface PUBLIC + $ + $ +) +ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL") + + +add_library(joint_saturation_limiter SHARED + src/joint_saturation_limiter.cpp +) +target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) +target_include_directories(joint_saturation_limiter PUBLIC + $ + $ +) +ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(generate_parameter_library REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) + target_link_libraries(joint_limits_rosparam_test joint_limits) + + ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) + target_link_libraries(joint_limits_urdf_test joint_limits) + + add_launch_test(test/joint_limits_rosparam.launch.py) + install( + TARGETS joint_limits_rosparam_test + DESTINATION lib/joint_limits + ) + install( + FILES test/joint_limits_rosparam.yaml test/joint_saturation_limiter_param.yaml + DESTINATION share/joint_limits/test + ) + + add_rostest_with_parameters_gmock(test_joint_saturation_limiter test/test_joint_saturation_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/joint_saturation_limiter_param.yaml + ) + target_include_directories(test_joint_saturation_limiter PRIVATE include) + target_link_libraries(test_joint_saturation_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_saturation_limiter + pluginlib + rclcpp +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) endif() @@ -57,7 +148,14 @@ install( DIRECTORY include/ DESTINATION include/joint_limits ) +<<<<<<< HEAD install(TARGETS joint_limits +======= +install(TARGETS + joint_limits + joint_limiter_interface + joint_saturation_limiter +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -65,6 +163,7 @@ install(TARGETS joint_limits ) ament_export_targets(export_joint_limits HAS_LIBRARY_TARGET) +<<<<<<< HEAD ament_export_dependencies( rclcpp rclcpp_lifecycle @@ -74,5 +173,8 @@ ament_export_include_directories( include ) +======= +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_package() ament_generate_version_header(${PROJECT_NAME}) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..17bffed021 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,249 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" +#include "joint_limits/visibility_control.h" +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_limits +{ +using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; + +template +class JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; + + JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; + + /// Initialization of every JointLimiter. + /** + * Initialization of JointLimiter for defined joints with their names. + * Robot description topic provides a topic name where URDF of the robot can be found. + * This is needed to use joint limits from URDF (not implemented yet!). + * Override this method only if initialization and reading joint limits should be adapted. + * Otherwise, initialize your custom limiter in `on_limit` method. + * + * \param[in] joint_names names of joints where limits should be applied. + * \param[in] param_itf node parameters interface object to access parameters. + * \param[in] logging_itf node logging interface to log if error happens. + * \param[in] robot_description_topic string of a topic where robot description is accessible. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + const std::string & robot_description_topic = "/robot_description") + { + number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; + joint_limits_.resize(number_of_joints_); + node_param_itf_ = param_itf; + node_logging_itf_ = logging_itf; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + for (size_t i = 0; i < number_of_joints_; ++i) + { + if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_)) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str()); + result = false; + break; + } + if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i])) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i, + joint_names[i].c_str(), joint_limits_[i].to_string().c_str()); + } + updated_limits_.writeFromNonRT(joint_limits_); + + auto on_parameter_event_callback = [this](const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult set_parameters_result; + set_parameters_result.successful = true; + + std::vector updated_joint_limits = joint_limits_; + bool changed = false; + + for (size_t i = 0; i < number_of_joints_; ++i) + { + changed |= joint_limits::check_for_limits_update( + joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]); + } + + if (changed) + { + updated_limits_.writeFromNonRT(updated_joint_limits); + RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!"); + } + + return set_parameters_result; + }; + + parameter_callback_ = + node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); + + if (result) + { + result = on_init(); + } + + (void)robot_description_topic; // avoid linters output + + return result; + } + + /** + * Wrapper init method that accepts pointer to the Node. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, node->get_node_parameters_interface(), node->get_node_logging_interface(), + robot_description_topic); + } + + /** + * Wrapper init method that accepts pointer to the LifecycleNode. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), robot_description_topic); + } + + JOINT_LIMITS_PUBLIC virtual bool configure(const JointLimitsStateDataType & current_joint_states) + { + return on_configure(current_joint_states); + } + + /** \brief Enforce joint limits to desired joint state for multiple physical quantities. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) + { + joint_limits_ = *(updated_limits_.readFromRT()); + return on_enforce(current_joint_states, desired_joint_states, dt); + } + + /** \brief Enforce joint limits to desired joint state for single physical quantity. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector & desired_joint_states) + { + joint_limits_ = *(updated_limits_.readFromRT()); + return on_enforce(desired_joint_states); + } + +protected: + /** \brief Method is realized by an implementation. + * + * Implementation-specific initialization of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_init() = 0; + + /** \brief Method is realized by an implementation. + * + * Implementation-specific configuration of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_configure( + const JointLimitsStateDataType & current_joint_states) = 0; + + /** \brief Method is realized by an implementation. + * + * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent + * physical quantities. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; + + /** \brief Method is realized by an implementation. + * + * Filter-specific implementation of the joint limits enforce algorithm for single physical + * quantity. + * This method might use "effort" limits since they are often used as wild-card. + * Check the documentation of the exact implementation for more details. + * + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector & desired_joint_states) = 0; + + size_t number_of_joints_; + std::vector joint_names_; + std::vector joint_limits_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_; + +private: + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; + realtime_tools::RealtimeBuffer> updated_limits_; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index fffab7a841..f24faea590 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -38,11 +38,19 @@ struct JointLimits max_position(std::numeric_limits::quiet_NaN()), max_velocity(std::numeric_limits::quiet_NaN()), max_acceleration(std::numeric_limits::quiet_NaN()), +<<<<<<< HEAD +======= + max_deceleration(std::numeric_limits::quiet_NaN()), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) max_jerk(std::numeric_limits::quiet_NaN()), max_effort(std::numeric_limits::quiet_NaN()), has_position_limits(false), has_velocity_limits(false), has_acceleration_limits(false), +<<<<<<< HEAD +======= + has_deceleration_limits(false), +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) has_jerk_limits(false), has_effort_limits(false), angle_wraparound(false) @@ -53,12 +61,20 @@ struct JointLimits double max_position; double max_velocity; double max_acceleration; +<<<<<<< HEAD +======= + double max_deceleration; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) double max_jerk; double max_effort; bool has_position_limits; bool has_velocity_limits; bool has_acceleration_limits; +<<<<<<< HEAD +======= + bool has_deceleration_limits; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) bool has_jerk_limits; bool has_effort_limits; bool angle_wraparound; @@ -73,6 +89,11 @@ struct JointLimits << max_velocity << "]\n"; ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false") << " [" << max_acceleration << "]\n"; +<<<<<<< HEAD +======= + ss_output << " has deceleration limits: " << (has_deceleration_limits ? "true" : "false") + << " [" << max_deceleration << "]\n"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << " [" << max_jerk << "]\n"; ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << " [" diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index f284a8db3a..c8ee887e26 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -19,9 +19,16 @@ #include #include +<<<<<<< HEAD #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" +======= +#include + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/node.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace // utilities @@ -62,6 +69,11 @@ namespace joint_limits * max_velocity: double * has_acceleration_limits: bool * max_acceleration: double +<<<<<<< HEAD +======= + * has_deceleration_limits: bool + * max_deceleration: double +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * has_jerk_limits: bool * max_jerk: double * has_effort_limits: bool @@ -95,12 +107,21 @@ inline bool declare_parameters( param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_velocity_limits", false); auto_declare( +<<<<<<< HEAD param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); auto_declare( +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false); auto_declare( param_itf, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); +<<<<<<< HEAD +======= + auto_declare(param_itf, param_base_name + ".has_deceleration_limits", false); + auto_declare( + param_itf, param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) auto_declare(param_itf, param_base_name + ".has_jerk_limits", false); auto_declare( param_itf, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); @@ -172,6 +193,11 @@ inline bool declare_parameters( * max_velocity: double * has_acceleration_limits: bool * max_acceleration: double +<<<<<<< HEAD +======= + * has_deceleration_limits: bool + * max_deceleration: double +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * has_jerk_limits: bool * max_jerk: double * has_effort_limits: bool @@ -193,6 +219,11 @@ inline bool declare_parameters( * max_velocity: 2.0 * has_acceleration_limits: true * max_acceleration: 5.0 +<<<<<<< HEAD +======= + * has_deceleration_limits: true + * max_deceleration: 7.5 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * has_jerk_limits: true * max_jerk: 100.0 * has_effort_limits: true @@ -228,20 +259,32 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".min_position") && !param_itf->has_parameter(param_base_name + ".max_position") && !param_itf->has_parameter(param_base_name + ".has_velocity_limits") && +<<<<<<< HEAD !param_itf->has_parameter(param_base_name + ".min_velocity") && !param_itf->has_parameter(param_base_name + ".max_velocity") && !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && !param_itf->has_parameter(param_base_name + ".max_acceleration") && +======= + !param_itf->has_parameter(param_base_name + ".max_velocity") && + !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && + !param_itf->has_parameter(param_base_name + ".max_acceleration") && + !param_itf->has_parameter(param_base_name + ".has_deceleration_limits") && + !param_itf->has_parameter(param_base_name + ".max_deceleration") && +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) !param_itf->has_parameter(param_base_name + ".has_jerk_limits") && !param_itf->has_parameter(param_base_name + ".max_jerk") && !param_itf->has_parameter(param_base_name + ".has_effort_limits") && !param_itf->has_parameter(param_base_name + ".max_effort") && +<<<<<<< HEAD !param_itf->has_parameter(param_base_name + ".angle_wraparound") && !param_itf->has_parameter(param_base_name + ".has_soft_limits") && !param_itf->has_parameter(param_base_name + ".k_position") && !param_itf->has_parameter(param_base_name + ".k_velocity") && !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) +======= + !param_itf->has_parameter(param_base_name + ".angle_wraparound")) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) { RCLCPP_ERROR( logging_itf->get_logger(), @@ -316,6 +359,27 @@ inline bool get_joint_limits( } } +<<<<<<< HEAD +======= + // Deceleration limits + if (param_itf->has_parameter(param_base_name + ".has_deceleration_limits")) + { + limits.has_deceleration_limits = + param_itf->get_parameter(param_base_name + ".has_deceleration_limits").as_bool(); + if ( + limits.has_deceleration_limits && + param_itf->has_parameter(param_base_name + ".max_deceleration")) + { + limits.max_deceleration = + param_itf->get_parameter(param_base_name + ".max_deceleration").as_double(); + } + else + { + limits.has_deceleration_limits = false; + } + } + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Jerk limits if (param_itf->has_parameter(param_base_name + ".has_jerk_limits")) { @@ -392,6 +456,221 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), limits); } +<<<<<<< HEAD +======= +/** + * Check if any of updated parameters are related to JointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the JointLimits + * structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated JointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + JointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + // update first numerical values to make later checks for "has" limits members + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".min_position") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_position") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_velocity") + { + changed = updated_limits.max_velocity != parameter.get_value(); + updated_limits.max_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_acceleration") + { + changed = updated_limits.max_acceleration != parameter.get_value(); + updated_limits.max_acceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_deceleration") + { + changed = updated_limits.max_deceleration != parameter.get_value(); + updated_limits.max_deceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_jerk") + { + changed = updated_limits.max_jerk != parameter.get_value(); + updated_limits.max_jerk = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_effort") + { + changed = updated_limits.max_effort != parameter.get_value(); + updated_limits.max_effort = parameter.get_value(); + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".has_position_limits") + { + updated_limits.has_position_limits = parameter.get_value(); + if (updated_limits.has_position_limits) + { + if (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if 'min_position' " + "and 'max_position' are not set or not have valid double values."); + updated_limits.has_position_limits = false; + } + else if (updated_limits.min_position >= updated_limits.max_position) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if not " + "'min_position' < 'max_position'"); + updated_limits.has_position_limits = false; + } + else + { + changed = true; + } + } + } + else if (param_name == param_base_name + ".has_velocity_limits") + { + updated_limits.has_velocity_limits = parameter.get_value(); + if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' " + "and 'max_velocity' are not set or not have valid double values."); + updated_limits.has_velocity_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_acceleration_limits") + { + updated_limits.has_acceleration_limits = parameter.get_value(); + if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if " + "'max_acceleration' is not set or not have valid double values."); + updated_limits.has_acceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_deceleration_limits") + { + updated_limits.has_deceleration_limits = parameter.get_value(); + if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if " + "'max_deceleration' is not set or not have valid double values."); + updated_limits.has_deceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_jerk_limits") + { + updated_limits.has_jerk_limits = parameter.get_value(); + if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set " + "or not have valid double values."); + updated_limits.has_jerk_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_effort_limits") + { + updated_limits.has_effort_limits = parameter.get_value(); + if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not " + "set or not have valid double values."); + updated_limits.has_effort_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".angle_wraparound") + { + updated_limits.angle_wraparound = parameter.get_value(); + if (updated_limits.angle_wraparound && updated_limits.has_position_limits) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if " + "'has_position_limits' flag is set."); + updated_limits.angle_wraparound = false; + } + else + { + changed = true; + } + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN( + logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s", + e.what()); + } + } + + return changed; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) /// Populate a SoftJointLimits instance from the ROS parameter server. /** * It is assumed that the parameter structure is the following: @@ -521,6 +800,88 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), soft_limits); } +<<<<<<< HEAD +======= +/** + * Check if any of updated parameters are related to SoftJointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the + * SoftJointLimits structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + SoftJointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".has_soft_limits") + { + if (!parameter.get_value()) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!"); + return false; + } + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".k_position") + { + changed = updated_limits.k_position != parameter.get_value(); + updated_limits.k_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".k_velocity") + { + changed = updated_limits.k_velocity != parameter.get_value(); + updated_limits.k_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_lower_limit") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_upper_limit") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + return changed; +} + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace joint_limits #endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..cdcbaf9c9d --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 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. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ + +#include "joint_limits/joint_limits.hpp" +#include "urdf_model/joint.h" + +namespace joint_limits +{ + +/** + * \brief Populate a JointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \return True if \e urdf_joint has a valid limits specification, false otherwise. + */ +inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) +{ + if (!urdf_joint || !urdf_joint->limits) + { + return false; + } + + limits.has_position_limits = + urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; + if (limits.has_position_limits) + { + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + } + + if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + limits.angle_wraparound = true; + } + + limits.has_velocity_limits = true; + limits.max_velocity = std::abs(urdf_joint->limits->velocity); + + limits.has_acceleration_limits = false; + + limits.has_effort_limits = true; + limits.max_effort = std::abs(urdf_joint->limits->effort); + + return true; +} + +/** + * \brief Populate a SoftJointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] soft_limits Where URDF soft joint limit data gets written into. + * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + */ +inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) +{ + if (!urdf_joint || !urdf_joint->safety) + { + return false; + } + + soft_limits.min_position = urdf_joint->safety->soft_lower_limit; + soft_limits.max_position = urdf_joint->safety->soft_upper_limit; + soft_limits.k_position = urdf_joint->safety->k_position; + soft_limits.k_velocity = urdf_joint->safety->k_velocity; + + return true; +} +} // namespace joint_limits +#endif // JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp new file mode 100644 index 0000000000..a69cc2791c --- /dev/null +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -0,0 +1,102 @@ +// Copyright (c) 2024, PickNik Inc. +// +// 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. + +/// \author Dr. Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/** + * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values + * to its minimal and maximal allowed values. Since the position, velocity and accelerations are + * variables in physical relation, it might be that some values are limited lower then specified + * limit. For example, if a joint is close to its position limit, velocity and acceleration will be + * reduced accordingly. + */ +template +class JointSaturationLimiter : public JointLimiterInterface +{ +public: + /** \brief Constructor */ + JOINT_LIMITS_PUBLIC JointSaturationLimiter(); + + /** \brief Destructor */ + JOINT_LIMITS_PUBLIC ~JointSaturationLimiter(); + + JOINT_LIMITS_PUBLIC bool on_init() override { return true; } + + JOINT_LIMITS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override + { + return true; + } + + /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. + * Class implements this method accepting following data types: + * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration; + * + * Implementation of saturation approach for joints with position, velocity or acceleration limits + * and values. First, position limits are checked to adjust desired velocity accordingly, then + * velocity and finally acceleration. + * The method support partial existence of limits, e.g., missing position limits for continuous + * joins. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; + + /** \brief Enforce joint limits to desired arbitrary physical quantity. + * Additional, commonly used method for enforcing limits by clamping desired input value. + * The limit is defined in effort limits of the `joint::limits/JointLimit` structure. + * + * If `has_effort_limits` is set to false, the limits will be not enforced to a joint. + * + * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the + * limits. \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce(std::vector & desired_joint_states) override; + +private: + rclcpp::Clock::SharedPtr clock_; +}; + +template +JointSaturationLimiter::JointSaturationLimiter() : JointLimiterInterface() +{ + clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); +} + +template +JointSaturationLimiter::~JointSaturationLimiter() +{ +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h new file mode 100644 index 0000000000..54edbc15f3 --- /dev/null +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_BUILDING_DLL +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT +#else +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT +#endif +#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#else +#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#endif +#define JOINT_LIMITS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml new file mode 100644 index 0000000000..a49d88fbc9 --- /dev/null +++ b/joint_limits/joint_limiters.xml @@ -0,0 +1,11 @@ + + + + + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + + + + diff --git a/joint_limits/package.xml b/joint_limits/package.xml index bc2a840efd..2f406349fa 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,10 +1,18 @@ joint_limits +<<<<<<< HEAD 2.45.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar Denis Štogl +======= + 4.21.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 + Denis Štogl +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Apache License 2.0 @@ -15,12 +23,28 @@ ament_cmake ament_cmake_gen_version_h +<<<<<<< HEAD rclcpp rclcpp_lifecycle launch_ros launch_testing_ament_cmake ament_cmake_gtest +======= + backward_ros + pluginlib + realtime_tools + rclcpp + rclcpp_lifecycle + trajectory_msgs + urdf + + ament_cmake_gmock + ament_cmake_gtest + generate_parameter_library + launch_ros + launch_testing_ament_cmake +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_cmake diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp new file mode 100644 index 0000000000..aea0e6704d --- /dev/null +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -0,0 +1,21 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Dr. Denis Stogl + +#include "joint_limits/joint_limiter_interface.hpp" + +namespace joint_limits +{ +} // namespace joint_limits diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp new file mode 100644 index 0000000000..f2a3adae1e --- /dev/null +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -0,0 +1,477 @@ +// Copyright (c) 2024, PickNik Inc. +// +// 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. + +/// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck + +#include "joint_limits/joint_saturation_limiter.hpp" + +#include + +#include "rclcpp/duration.hpp" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ +template <> +bool JointSaturationLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + bool limits_enforced = false; + + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) + { + return false; + } + + // TODO(gwalck) compute if the max are not implicitly violated with the given dt + // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4 + // velocity max is implicitly already violated due to max_acc * dt > 2.0 + + // check for required inputs combination + bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); + bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); + bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_); + bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); + bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); + + // pos state and vel or pos cmd is required, vel state is optional + if (!(has_current_position && (has_desired_position || has_desired_velocity))) + { + return false; + } + + if (!has_current_velocity) + { + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(number_of_joints_, 0.0); + } + + // TODO(destogl): please check if we get too much malloc from this initialization, + // if so then we should use members instead local variables and initialize them in other method + std::vector desired_pos(number_of_joints_); + std::vector desired_vel(number_of_joints_); + std::vector desired_acc(number_of_joints_); + std::vector expected_vel(number_of_joints_); + std::vector expected_pos(number_of_joints_); + + // limits triggered + std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; + + bool braking_near_position_limit_triggered = false; + + for (size_t index = 0; index < number_of_joints_; ++index) + { + if (has_desired_position) + { + desired_pos[index] = desired_joint_states.positions[index]; + } + if (has_desired_velocity) + { + desired_vel[index] = desired_joint_states.velocities[index]; + } + if (has_desired_acceleration) + { + desired_acc[index] = desired_joint_states.accelerations[index]; + } + + if (has_desired_position) + { + // limit position + if (joint_limits_[index].has_position_limits) + { + // clamp input pos_cmd + auto pos = std::clamp( + desired_pos[index], joint_limits_[index].min_position, joint_limits_[index].max_position); + if (pos != desired_pos[index]) + { + desired_pos[index] = pos; + limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might + // get jumps in the velocity based on the system's dynamics. Position limit clamping is done + // below once again. + const double position_difference = desired_pos[index] - current_joint_states.positions[index]; + if ( + std::fabs(position_difference) > VALUE_CONSIDERED_ZERO && + std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO) + { + desired_vel[index] = position_difference / dt_seconds; + } + } + + // limit velocity + if (joint_limits_[index].has_velocity_limits) + { + // if desired velocity is not defined calculate it from positions + if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index])) + { + desired_vel[index] = + (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + } + // clamp input vel_cmd + if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity) + { + desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); + limited_jnts_vel.emplace_back(joint_names_[index]); + limits_enforced = true; + + // recompute pos_cmd if needed + if (has_desired_position) + { + desired_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; + } + + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } + } + + // limit acceleration + if ( + joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) + { + // if(has_current_velocity) + if (1) // for now use a zero velocity if not provided + { + // limiting acc or dec function + auto apply_acc_or_dec_limit = [&]( + const double max_acc_or_dec, std::vector & acc, + std::vector & limited_jnts) -> bool + { + if (std::fabs(acc[index]) > max_acc_or_dec) + { + acc[index] = std::copysign(max_acc_or_dec, acc[index]); + limited_jnts.emplace_back(joint_names_[index]); + limits_enforced = true; + return true; + } + else + { + return false; + } + }; + + // if desired acceleration if not provided compute it from desired_vel and vel_state + if ( + std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) + { + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } + + // check if decelerating - if velocity is changing toward 0 + bool deceleration_limit_applied = false; + bool limit_applied = false; + if ( + (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) || + (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0)) + { + // limit deceleration + if (joint_limits_[index].has_deceleration_limits) + { + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); + deceleration_limit_applied = true; + } + } + + // limit acceleration (fallback to acceleration if no deceleration limits) + if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) + { + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); + } + + if (limit_applied) + { + // vel_cmd from integration of desired_acc, needed even if no vel output + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + if (has_desired_position) + { + // pos_cmd from from double integration of desired_acc + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + } + } + // else we cannot compute acc, so not limiting it + } + + // plan ahead for position limits + if (joint_limits_[index].has_position_limits) + { + if (has_desired_velocity && !has_desired_position) + { + // Check immediate next step when using vel_cmd only, other cases already handled + // integrate pos + expected_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; + // if expected_pos over limit + auto pos = std::clamp( + expected_pos[index], joint_limits_[index].min_position, + joint_limits_[index].max_position); + if (pos != expected_pos[index]) + { + // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full + // deceleration in any case limit pos to max + expected_pos[index] = pos; + // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be + // zero) + desired_vel[index] = + (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; + limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + + // Check that stopping distance is within joint limits + // Slow down all joints at maximum decel if any don't have stopping distance within joint + // limits + + // delta_x = (v2*v2 - v1*v1) / (2*a) + // stopping_distance = (- v1*v1) / (2*max_acceleration) + // Here we assume we will not trigger velocity limits while maximally decelerating. + // This is a valid assumption if we are not currently at a velocity limit since we are just + // coming to a rest. + double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds); + if (joint_limits_[index].has_deceleration_limits) + { + stopping_deccel = joint_limits_[index].max_deceleration; + } + else if (joint_limits_[index].has_acceleration_limits) + { + stopping_deccel = joint_limits_[index].max_acceleration; + } + + double stopping_distance = + std::fabs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); + // compute stopping duration at stopping_deccel + double stopping_duration = std::fabs((desired_vel[index]) / (stopping_deccel)); + + // Check that joint limits are beyond stopping_distance and desired_velocity is towards + // that limit + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + stopping_distance)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + stopping_distance))) + { + limited_jnts_pos.emplace_back(joint_names_[index]); + braking_near_position_limit_triggered = true; + limits_enforced = true; + } + else + { + // compute the travel_distance at new desired velocity, with best case duration + // stopping_duration + double motion_after_stopping_duration = desired_vel[index] * stopping_duration; + // re-check what happens if we don't slow down + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + motion_after_stopping_duration)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + motion_after_stopping_duration))) + { + limited_jnts_pos.emplace_back(joint_names_[index]); + braking_near_position_limit_triggered = true; + limits_enforced = true; + } + // else no need to slow down. in worse case we won't hit the limit at current velocity + } + } + } + + // update variables according to triggers + if (braking_near_position_limit_triggered) + { + // this limit applies to all joints even if a single one is triggered + for (size_t index = 0; index < number_of_joints_; ++index) + { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds; + if (joint_limits_[index].has_deceleration_limits) + { + desired_acc[index] = std::copysign( + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_deceleration), + desired_acc[index]); + } + else if (joint_limits_[index].has_acceleration_limits) + { + desired_acc[index] = std::copysign( + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_acceleration), + desired_acc[index]); + } + + // Recompute velocity and position + if (has_desired_velocity) + { + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + } + if (has_desired_position) + { + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + } + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() + << "] would exceed position limits" + " if continuing at current state, limiting all joints"); + } + + // display limitations + + // if position limiting + if (limited_jnts_pos.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); + } + + if (limited_jnts_vel.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_vel) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); + } + + if (limited_jnts_acc.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_acc) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); + } + + if (limited_jnts_dec.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_dec) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting"); + } + + if (has_desired_position) + { + desired_joint_states.positions = desired_pos; + } + if (has_desired_velocity) + { + desired_joint_states.velocities = desired_vel; + } + if (has_desired_acceleration) + { + desired_joint_states.accelerations = desired_acc; + } + + return limits_enforced; +} + +template <> +bool JointSaturationLimiter::on_enforce(std::vector & desired_joint_states) +{ + std::vector limited_joints_effort; + bool limits_enforced = false; + + bool has_cmd = (desired_joint_states.size() == number_of_joints_); + if (!has_cmd) + { + return false; + } + + for (size_t index = 0; index < number_of_joints_; ++index) + { + if (joint_limits_[index].has_effort_limits) + { + double max_effort = joint_limits_[index].max_effort; + if (std::fabs(desired_joint_states[index]) > max_effort) + { + desired_joint_states[index] = std::copysign(max_effort, desired_joint_states[index]); + limited_joints_effort.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + } + + if (limited_joints_effort.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_joints_effort) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed effort limits, limiting"); + } + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + joint_limits::JointSaturationLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits/test/joint_limits_rosparam.yaml b/joint_limits/test/joint_limits_rosparam.yaml index 521fbf93f8..fafcf34af9 100644 --- a/joint_limits/test/joint_limits_rosparam.yaml +++ b/joint_limits/test/joint_limits_rosparam.yaml @@ -10,6 +10,11 @@ JointLimitsRosparamTestNode: max_velocity: 2.0 has_acceleration_limits: true max_acceleration: 5.0 +<<<<<<< HEAD +======= + has_deceleration_limits: true + max_deceleration: 7.5 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) has_jerk_limits: true max_jerk: 100.0 has_effort_limits: true @@ -26,6 +31,10 @@ JointLimitsRosparamTestNode: has_position_limits: true has_velocity_limits: true has_acceleration_limits: true +<<<<<<< HEAD +======= + has_deceleration_limits: true +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) has_jerk_limits: true has_effort_limits: true @@ -35,6 +44,10 @@ JointLimitsRosparamTestNode: max_position: 1.0 max_velocity: 2.0 max_acceleration: 5.0 +<<<<<<< HEAD +======= + max_deceleration: 7.5 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) max_jerk: 100.0 max_effort: 20.0 @@ -43,6 +56,10 @@ JointLimitsRosparamTestNode: has_position_limits: false has_velocity_limits: false has_acceleration_limits: false +<<<<<<< HEAD +======= + has_deceleration_limits: false +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) has_jerk_limits: false has_effort_limits: false angle_wraparound: true # should be accepted, has no position limits diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index c5ddb8f585..09eb19b2ec 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -14,12 +14,18 @@ /// \author Adolfo Rodriguez Tsouroukdissian +<<<<<<< HEAD #include #include "gtest/gtest.h" #include "joint_limits/joint_limits_rosparam.hpp" #include "rclcpp/rclcpp.hpp" +======= +#include + +#include "joint_limits/joint_limits_rosparam.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "rclcpp_lifecycle/lifecycle_node.hpp" class JointLimitsRosParamTest : public ::testing::Test @@ -54,6 +60,11 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_TRUE(std::isnan(limits.max_velocity)); EXPECT_FALSE(limits.has_acceleration_limits); EXPECT_TRUE(std::isnan(limits.max_acceleration)); +<<<<<<< HEAD +======= + EXPECT_FALSE(limits.has_deceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_deceleration)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(limits.has_jerk_limits); EXPECT_TRUE(std::isnan(limits.max_jerk)); EXPECT_FALSE(limits.has_effort_limits); @@ -76,6 +87,11 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_TRUE(std::isnan(limits.max_velocity)); EXPECT_FALSE(limits.has_acceleration_limits); EXPECT_TRUE(std::isnan(limits.max_acceleration)); +<<<<<<< HEAD +======= + EXPECT_FALSE(limits.has_deceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_deceleration)); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(limits.has_jerk_limits); EXPECT_TRUE(std::isnan(limits.max_jerk)); EXPECT_FALSE(limits.has_effort_limits); @@ -100,6 +116,12 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_TRUE(limits.has_acceleration_limits); EXPECT_EQ(5.0, limits.max_acceleration); +<<<<<<< HEAD +======= + EXPECT_TRUE(limits.has_deceleration_limits); + EXPECT_EQ(7.5, limits.max_deceleration); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(limits.has_jerk_limits); EXPECT_EQ(100.0, limits.max_jerk); @@ -120,6 +142,10 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); EXPECT_FALSE(limits.has_acceleration_limits); +<<<<<<< HEAD +======= + EXPECT_FALSE(limits.has_deceleration_limits); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(limits.has_jerk_limits); EXPECT_FALSE(limits.has_effort_limits); } @@ -134,6 +160,10 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); EXPECT_FALSE(limits.has_acceleration_limits); +<<<<<<< HEAD +======= + EXPECT_FALSE(limits.has_deceleration_limits); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(limits.has_jerk_limits); EXPECT_FALSE(limits.has_effort_limits); } @@ -147,6 +177,10 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_TRUE(limits.has_position_limits); EXPECT_TRUE(limits.has_velocity_limits); EXPECT_TRUE(limits.has_acceleration_limits); +<<<<<<< HEAD +======= + EXPECT_TRUE(limits.has_deceleration_limits); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(limits.has_jerk_limits); EXPECT_TRUE(limits.has_effort_limits); @@ -156,6 +190,10 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); EXPECT_FALSE(limits.has_acceleration_limits); +<<<<<<< HEAD +======= + EXPECT_FALSE(limits.has_deceleration_limits); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(limits.has_jerk_limits); EXPECT_FALSE(limits.has_effort_limits); EXPECT_TRUE(limits.angle_wraparound); @@ -191,6 +229,12 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_FALSE(limits.has_acceleration_limits); EXPECT_TRUE(std::isnan(limits.max_acceleration)); +<<<<<<< HEAD +======= + EXPECT_FALSE(limits.has_deceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_deceleration)); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_FALSE(limits.has_jerk_limits); EXPECT_TRUE(std::isnan(limits.max_jerk)); @@ -317,6 +361,12 @@ TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_joint_limits_node) EXPECT_TRUE(limits.has_acceleration_limits); EXPECT_EQ(5.0, limits.max_acceleration); +<<<<<<< HEAD +======= + EXPECT_TRUE(limits.has_deceleration_limits); + EXPECT_EQ(7.5, limits.max_deceleration); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(limits.has_jerk_limits); EXPECT_EQ(100.0, limits.max_jerk); @@ -356,6 +406,12 @@ TEST_F(JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_joint_limi EXPECT_TRUE(limits.has_acceleration_limits); EXPECT_EQ(5.0, limits.max_acceleration); +<<<<<<< HEAD +======= + EXPECT_TRUE(limits.has_deceleration_limits); + EXPECT_EQ(7.5, limits.max_deceleration); + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) EXPECT_TRUE(limits.has_jerk_limits); EXPECT_EQ(100.0, limits.max_jerk); diff --git a/joint_limits/test/joint_limits_urdf_test.cpp b/joint_limits/test/joint_limits_urdf_test.cpp new file mode 100644 index 0000000000..82c7e13613 --- /dev/null +++ b/joint_limits/test/joint_limits_urdf_test.cpp @@ -0,0 +1,175 @@ +// Copyright 2024 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. + +/// \author Adolfo Rodriguez Tsouroukdissian +#include + +#include "joint_limits/joint_limits_urdf.hpp" + +using std::string; + +class JointLimitsUrdfTest : public ::testing::Test +{ +public: + JointLimitsUrdfTest() + { + urdf_limits.reset(new urdf::JointLimits); + urdf_limits->effort = 8.0; + urdf_limits->velocity = 2.0; + urdf_limits->lower = -1.0; + urdf_limits->upper = 1.0; + + urdf_safety.reset(new urdf::JointSafety); + urdf_safety->k_position = 20.0; + urdf_safety->k_velocity = 40.0; + urdf_safety->soft_lower_limit = -0.8; + urdf_safety->soft_upper_limit = 0.8; + + urdf_joint.reset(new urdf::Joint); + urdf_joint->limits = urdf_limits; + urdf_joint->safety = urdf_safety; + + urdf_joint->type = urdf::Joint::UNKNOWN; + } + +protected: + urdf::JointLimitsSharedPtr urdf_limits; + urdf::JointSafetySharedPtr urdf_safety; + urdf::JointSharedPtr urdf_joint; +}; + +TEST_F(JointLimitsUrdfTest, GetJointLimits) +{ + // Unset URDF joint + { + joint_limits::JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Unset URDF limits + { + joint_limits::JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Valid URDF joint, CONTINUOUS type + { + urdf_joint->type = urdf::Joint::CONTINUOUS; + + joint_limits::JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, REVOLUTE type + { + urdf_joint->type = urdf::Joint::REVOLUTE; + + joint_limits::JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, PRISMATIC type + { + urdf_joint->type = urdf::Joint::PRISMATIC; + + joint_limits::JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } +} + +TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) +{ + // Unset URDF joint + { + joint_limits::SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Unset URDF limits + { + joint_limits::SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Valid URDF joint + { + joint_limits::SoftJointLimits soft_limits; + EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); + + // Soft limits + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/joint_limits/test/joint_saturation_limiter_param.yaml b/joint_limits/test/joint_saturation_limiter_param.yaml new file mode 100644 index 0000000000..f025421519 --- /dev/null +++ b/joint_limits/test/joint_saturation_limiter_param.yaml @@ -0,0 +1,57 @@ +joint_saturation_limiter: + ros__parameters: + joint_limits: + # Get full specification from parameter server + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: true + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + + foo_joint_no_effort: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: false + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + +joint_saturation_limiter_nodeclimit: + ros__parameters: + joint_limits: + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp new file mode 100644 index 0000000000..6ecbd46d71 --- /dev/null +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -0,0 +1,569 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Dr. Denis Stogl, Guillaume Walck + +#include "test_joint_saturation_limiter.hpp" + +TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSaturationLimiter loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +// NOTE: We accept also if there is no limit defined for a joint. +TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); + } +} + +TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + // test no desired interface + desired_joint_states_.positions.clear(); + desired_joint_states_.velocities.clear(); + // currently not handled desired_joint_states_.accelerations.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no position interface + current_joint_states_.positions.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // also fail if out of limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no vel interface + current_joint_states_.velocities.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + // also fail if out of limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.0, // vel unchanged + 1.0 // acc = vel / 1.0 + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well + desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.5, // vel unchanged + 2.9 // acc = vel / 1.0 + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // pos/vel cmd ifs + current_joint_states_.positions[0] = -2.0; + desired_joint_states_.positions[0] = 1.0; + // desired velocity exceeds although correctly computed from pos derivative + desired_joint_states_.velocities[0] = 3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.0, // pos = pos + max_vel * dt + 2.0, // vel limited to max_vel + 2.0 / 1.0 // acc set to vel change/DT + ); + + // check opposite velocity direction (sign copy) + current_joint_states_.positions[0] = 2.0; + desired_joint_states_.positions[0] = -1.0; + // desired velocity exceeds although correctly computed from pos derivative + desired_joint_states_.velocities[0] = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.0, // pos = pos - max_vel * dt + -2.0, // vel limited to -max_vel + -2.0 / 1.0 // acc set to vel change/DT + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // desired pos leads to vel exceeded (4.0 / sec instead of 2.0) + desired_joint_states_.positions[0] = 4.0; + // no vel cmd (will lead to internal computation of velocity) + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + + // check opposite position and direction + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = -1.9; + desired_joint_states_.positions[0] = -4.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // vel cmd ifs + current_joint_states_.positions[0] = -2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // no pos cmd + desired_joint_states_.positions.clear(); + // desired velocity exceeds + desired_joint_states_.velocities[0] = 3.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + + // check opposite velocity direction + current_joint_states_.positions[0] = 2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = -1.9; + // desired velocity exceeds + desired_joint_states_.velocities[0] = -3.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // desired pos exceeds + current_joint_states_.positions[0] = 5.0; + desired_joint_states_.positions[0] = 20.0; + // no velocity interface + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + ASSERT_EQ( + desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // using 0.05 because 1.0 sec invalidates the "small dt integration" + rclcpp::Duration period(0, 50000000); // 0.05 second + + // close to pos limit should reduce velocity and stop + current_joint_states_.positions[0] = 4.851; + current_joint_states_.velocities[0] = 1.5; + desired_joint_states_.positions[0] = 4.926; + desired_joint_states_.velocities[0] = 1.5; + + // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) + std::vector expected_ret = {true, true, true, false}; + for (auto i = 0u; i < 4; ++i) + { + auto previous_vel_request = desired_joint_states_.velocities[0]; + // expect limits applied until the end stop + ASSERT_EQ( + joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), + expected_ret[i]); + + ASSERT_LE( + desired_joint_states_.velocities[0], + previous_vel_request); // vel adapted to reach end-stop should be decreasing + // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit + // hence no max deceleration anymore... + ASSERT_LT( + desired_joint_states_.positions[0], + 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits + ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + + Integrate(period.seconds()); + + ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit + } + } +} + +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.11125, // pos = double integration from max acc with current state + 0.35, // vel limited to vel + max acc * dt + 5.0 // acc max acc + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = + 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_NEAR( + desired_joint_states_.positions[0], 0.111250, + COMMON_THRESHOLD); // pos = double integration from max acc with current state + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions.clear(); // = 0.125; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + // no pos cmd ifs + ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.3; + current_joint_states_.velocities[0] = 0.5; + desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.315625, // pos = double integration from max dec with current state + 0.125, // vel limited by vel - max dec * dt + -7.5 // acc limited by -max dec + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +{ + SetupNode("joint_saturation_limiter_nodeclimit"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 1.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.04375, // pos = double integration from max acc with current state + 0.75, // vel limited by vel-max acc * dt + -5.0 // acc limited to -max acc + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_there_are_effort_limits_expect_them_to_be_applyed) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // value not over limit + desired_joint_states_.effort[0] = 15.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + + // value over limit + desired_joint_states_.effort[0] = 21.0; + ASSERT_TRUE(joint_limiter_->enforce(desired_joint_states_.effort)); + ASSERT_EQ(desired_joint_states_.effort[0], 20.0); + } +} + +TEST_F(JointSaturationLimiterTest, when_there_are_no_effort_limits_expect_them_not_applyed) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init("foo_joint_no_effort"); + Configure(); + + // value not over limit + desired_joint_states_.effort[0] = 15.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + + // value over limit + desired_joint_states_.effort[0] = 21.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + ASSERT_EQ(desired_joint_states_.effort[0], 21.0); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp new file mode 100644 index 0000000000..20097ae591 --- /dev/null +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -0,0 +1,106 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#ifndef TEST_JOINT_SATURATION_LIMITER_HPP_ +#define TEST_JOINT_SATURATION_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +const double COMMON_THRESHOLD = 0.0011; + +#define CHECK_STATE_SINGLE_JOINT(tested_traj_point, idx, expected_pos, expected_vel, expected_acc) \ + EXPECT_NEAR(tested_traj_point.positions[idx], expected_pos, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); + +using JointLimiter = joint_limits::JointLimiterInterface; + +class JointSaturationLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) + { + node_name_ = node_name; + } + node_ = std::make_shared(node_name_); + } + + void Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + } + + void Init(const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + last_commanded_state_.positions.resize(num_joints_, 0.0); + last_commanded_state_.velocities.resize(num_joints_, 0.0); + last_commanded_state_.accelerations.resize(num_joints_, 0.0); + last_commanded_state_.effort.resize(num_joints_, 0.0); + desired_joint_states_ = last_commanded_state_; + current_joint_states_ = last_commanded_state_; + } + + void Configure() { joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + current_joint_states_.positions[0] += desired_joint_states_.velocities[0] * dt + + 0.5 * desired_joint_states_.accelerations[0] * dt * dt; + current_joint_states_.velocities[0] += desired_joint_states_.accelerations[0] * dt; + } + + JointSaturationLimiterTest() + : joint_limiter_type_("joint_limits/JointSaturationLimiter"), + joint_limiter_loader_( + "joint_limits", "joint_limits::JointLimiterInterface") + { + } + + void TearDown() override { node_.reset(); } + +protected: + std::string node_name_; + rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states_; + trajectory_msgs::msg::JointTrajectoryPoint current_joint_states_; +}; + +#endif // TEST_JOINT_SATURATION_LIMITER_HPP_ diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index c00419bb78..67bdea98b3 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -117,6 +118,161 @@ Changelog for package ros2_control 2.16.0 (2022-10-17) ------------------- +======= +4.21.0 (2024-12-06) +------------------- + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- + +4.18.0 (2024-10-07) +------------------- + +4.17.0 (2024-09-11) +------------------- + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + +4.13.0 (2024-07-08) +------------------- + +4.12.0 (2024-07-01) +------------------- +* Add custom rosdoc2 config for ros2_control metapackage (`#1484 `_) +* Contributors: Christoph Fröhlich + +4.11.0 (2024-05-14) +------------------- + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ + +4.8.0 (2024-03-27) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-03-02) +------------------ + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/ros2_control/CMakeLists.txt b/ros2_control/CMakeLists.txt index 28f50184e2..32481b6006 100644 --- a/ros2_control/CMakeLists.txt +++ b/ros2_control/CMakeLists.txt @@ -1,4 +1,8 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) +======= +cmake_minimum_required(VERSION 3.16) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) project(ros2_control) find_package(ament_cmake REQUIRED) diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 176434d1e2..4f74145cbe 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,11 @@ ros2_control +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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 eb3c8b1628..cb3f5deec9 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -119,6 +120,189 @@ Changelog for package ros2_control_test_assets 2.16.0 (2022-10-17) ------------------- +======= +4.21.0 (2024-12-06) +------------------- +* [Feature] Choose different read and write rate for the hardware components (`#1570 `_) +* Contributors: Sai Kishor Kothakota + +4.20.0 (2024-11-08) +------------------- +* Add Support for SDF (`#1763 `_) +* Contributors: Aarav Gupta + +4.19.0 (2024-10-26) +------------------- + +4.18.0 (2024-10-07) +------------------- +* Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) +* Contributors: Manuel Muth + +4.17.0 (2024-09-11) +------------------- +* [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) +* Contributors: Manuel Muth + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + +4.13.0 (2024-07-08) +------------------- +* [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) +* Remove mimic parameter from ros2_control tag (`#1553 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.12.0 (2024-07-01) +------------------- +* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 `_) +* [Feature] Hardware Components Grouping (`#1458 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.11.0 (2024-05-14) +------------------- +* Parse URDF soft_limits into the HardwareInfo structure (`#1488 `_) +* Contributors: adriaroig + +4.10.0 (2024-05-08) +------------------- +* Parse URDF joint hard limits into the HardwareInfo structure (`#1472 `_) +* Contributors: Sai Kishor Kothakota + +4.9.0 (2024-04-30) +------------------ +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + +4.8.0 (2024-03-27) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-03-02) +------------------ + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ +* [ResourceManager] adds test for uninitialized hardware (`#1243 `_) +* Contributors: Maximilian Schik + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- +* [ResourceManager] deactivate hardware from read/write return value (`#884 `_) +* Contributors: Felix Exner (fexner) + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- +* [URDF Parser] Allow empty urdf tag, e.g., parameter (`#1017 `_) +* Contributors: Felix Exner (fexner) + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Contributors: Bence Magyar + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index 47fe15a7f7..27f09c6beb 100644 --- a/ros2_control_test_assets/CMakeLists.txt +++ b/ros2_control_test_assets/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(ros2_control_test_assets) @@ -11,4 +12,30 @@ install( ament_export_include_directories( include ) +======= +cmake_minimum_required(VERSION 3.16) +project(ros2_control_test_assets LANGUAGES CXX) + +find_package(ament_cmake REQUIRED) + +add_library(ros2_control_test_assets INTERFACE) +target_compile_features(ros2_control_test_assets INTERFACE cxx_std_17) +target_include_directories(ros2_control_test_assets INTERFACE + $ + $ +) + +install( + DIRECTORY include/ + DESTINATION include/ros2_control_test_assets +) +install(TARGETS ros2_control_test_assets + EXPORT export_ros2_control_test_assets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_ros2_control_test_assets HAS_LIBRARY_TARGET) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_package() diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 816c229e43..91f2b5a3c0 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -15,8 +15,11 @@ #ifndef ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ #define ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) namespace ros2_control_test_assets { // 1. Industrial Robots with only one interface @@ -45,7 +48,11 @@ const auto valid_urdf_ros2_control_system_one_interface = )"; +<<<<<<< HEAD // 2. Industrial Robots with multiple interfaces (cannot be written at the same time) +======= +// 2.1 Industrial Robots with multiple interfaces (cannot be written at the same time) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // Note, joint parameters can be any string const auto valid_urdf_ros2_control_system_multi_interface = R"( @@ -86,6 +93,73 @@ const auto valid_urdf_ros2_control_system_multi_interface = )"; +<<<<<<< HEAD +======= +// 2.2 Industrial Robots with multiple interfaces (cannot be written at the same time) +// Additionally some of the Command-/StateInterfaces have parameters defined per interface +const auto valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters = + R"( + + + ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware + 2 + 2 + + + + -1 + 1 + 1.2 + 1 + 2 + + + -1.5 + 1.5 + 3.4 + 2 + 4 + + + -0.5 + 0.5 + + + 3 + 2 + + + 4 + 4 + + + 1.1.1.1 + 1234 + true + + + + -1 + 1 + 20 + int32_t + + + 21 + int32_t + + + + 21 + int32_t + + 192.168.178.123 + 4321 + + +)"; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) // 3. Industrial Robots with integrated sensor const auto valid_urdf_ros2_control_system_robot_with_sensor = R"( @@ -172,6 +246,10 @@ const auto valid_urdf_ros2_control_actuator_modular_robot = ros2_control_demo_hardware/PositionActuatorHardware +<<<<<<< HEAD +======= + Hardware Group +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1.23 3 @@ -186,6 +264,10 @@ const auto valid_urdf_ros2_control_actuator_modular_robot = ros2_control_demo_hardware/PositionActuatorHardware +<<<<<<< HEAD +======= + Hardware Group +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1.23 3 @@ -206,6 +288,10 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/VelocityActuatorHardware +<<<<<<< HEAD +======= + Hardware Group 1 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1.23 3 @@ -226,6 +312,10 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/VelocityActuatorHardware +<<<<<<< HEAD +======= + Hardware Group 2 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1.23 3 @@ -240,6 +330,10 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/PositionSensorHardware +<<<<<<< HEAD +======= + Hardware Group 1 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2 @@ -249,6 +343,10 @@ const auto valid_urdf_ros2_control_actuator_modular_robot_sensors = ros2_control_demo_hardware/PositionSensorHardware +<<<<<<< HEAD +======= + Hardware Group 2 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2 @@ -372,7 +470,13 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio = +<<<<<<< HEAD +======= + + 1.0 + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; @@ -398,6 +502,261 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +<<<<<<< HEAD +======= +// 12. Industrial Robots with integrated GPIO with few disabled limits in joints +const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + -2.0 + 2.0 + + + + + + 1.0 + + + + + + + + +)"; +const auto valid_urdf_ros2_control_system_robot_with_unavailable_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + + + + + 1.0 + + + -0.3 + 0.3 + + + +)"; +const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + + + + + + +)"; + +// Voltage Sensor only +const auto valid_urdf_ros2_control_voltage_sensor_only = + R"( + + + ros2_control_demo_hardware/SingleJointVoltageSensor + 2 + + + + + +)"; + +// Joint+Voltage Sensor +const auto valid_urdf_ros2_control_joint_voltage_sensor = + R"( + + + ros2_control_demo_hardware/SingleJointVoltageSensor + 2 + + + + + + + + +)"; + +const auto valid_urdf_ros2_control_dummy_actuator_only = + R"( + + + ros2_control_demo_hardware/VelocityActuatorHardware + 1.13 + 3 + + + + -1 + 1 + + + + + + transmission_interface/RotationToLinerTansmission + + 325.949 + + 1337 + + +)"; + +const auto valid_urdf_ros2_control_dummy_system_robot = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + +)"; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto valid_urdf_ros2_control_parameter_empty = R"( @@ -436,7 +795,11 @@ const auto invalid_urdf_ros2_control_missing_attribute = )"; +<<<<<<< HEAD const auto invalid_urdf_ros2_control_component_missing_class_type = +======= +const auto invalid_urdf_ros2_control_component_missing_plugin_name = +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) R"( @@ -467,7 +830,11 @@ const auto invalid_urdf_ros2_control_parameter_missing_name = )"; +<<<<<<< HEAD const auto invalid_urdf_ros2_control_component_class_type_empty = +======= +const auto invalid_urdf_ros2_control_component_plugin_name_empty = +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) R"( @@ -565,6 +932,24 @@ const auto invalid_urdf2_transmission_given_too_many_joints = )"; +<<<<<<< HEAD +======= +const auto invalid_urdf_ros2_control_system_with_command_fixed_joint = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + + + + +)"; + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) } // namespace ros2_control_test_assets #endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ 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 0525953091..c647be1211 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 @@ -23,10 +23,13 @@ namespace ros2_control_test_assets const auto urdf_head = R"( +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -90,6 +93,497 @@ const auto urdf_head = +<<<<<<< HEAD +======= + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_revolute_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_continuous_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_continuous_with_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_prismatic_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_mimic = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -122,12 +616,382 @@ const auto urdf_head = )"; -const auto urdf_tail = +const auto urdf_tail = + R"( + +)"; + +const auto hardware_resources = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + +<<<<<<< HEAD + + + + + +)"; + +const auto unitilizable_hardware_resources = + R"( + + + test_unitilizable_actuator +======= + + + + + +)"; + +const auto async_hardware_resources = + R"( + + + test_actuator +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + + + + + + + + +<<<<<<< HEAD + + + test_unitilizable_sensor +======= + + + test_sensor +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + 2 + 2 + + + + + +<<<<<<< HEAD + + + test_unitilizable_system +======= + + + test_system +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + 2 + 2 + + + + + + + + + + + + + + +<<<<<<< HEAD + + + +======= + + + + + +)"; + +const auto hardware_resources_with_different_rw_rates = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_with_negative_rw_rates = + R"( + + + test_actuator + + + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + + +)"; + +<<<<<<< HEAD +const auto hardware_resources_missing_state_keys = +======= +const auto hardware_resources_invalid_with_text_in_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + +const auto hardware_resources_invalid_out_of_range_in_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + +const auto uninitializable_hardware_resources = R"( - + + + test_uninitializable_actuator + + + + + + + + + + + test_uninitializable_sensor + 2 + 2 + + + + + + + + test_uninitializable_system + 2 + 2 + + + + + + + + + + + + + + + )"; -const auto hardware_resources = +const auto hardware_resources_not_existing_actuator_plugin = + R"( + + + test_actuator23 + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_not_existing_sensor_plugin = + R"( + + + test_actuator + + + + + + + + + + + test_sensor23 + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; +const auto hardware_resources_not_existing_system_plugin = R"( @@ -150,6 +1014,55 @@ const auto hardware_resources = + + + test_system23 + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_duplicated_component = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + test_system @@ -169,29 +1082,114 @@ const auto hardware_resources = - + + + +)"; + +const auto hardware_resources_actuator_initializaion_error = + R"( + + + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + )"; -const auto unitilizable_hardware_resources = +const auto hardware_resources_sensor_initializaion_error = +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) R"( - + - test_unitilizable_actuator + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + + test_system + 2 + 2 + + + + +<<<<<<< HEAD +======= + + + + + + + + +)"; + +const auto hardware_resources_system_initializaion_error = + R"( + + + test_actuator - - + - test_unitilizable_sensor + test_sensor 2 2 @@ -199,32 +1197,28 @@ const auto unitilizable_hardware_resources = - + - test_unitilizable_system + test_system 2 2 - - - +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) + - - - - - - + )"; +<<<<<<< HEAD +======= const auto hardware_resources_missing_state_keys = R"( @@ -235,6 +1229,7 @@ const auto hardware_resources_missing_state_keys = + @@ -246,6 +1241,7 @@ const auto hardware_resources_missing_state_keys = + @@ -256,17 +1252,20 @@ const auto hardware_resources_missing_state_keys = + - + + )"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto hardware_resources_missing_command_keys = R"( @@ -298,13 +1297,23 @@ const auto hardware_resources_missing_command_keys = +<<<<<<< HEAD +======= + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) )"; @@ -372,16 +1381,29 @@ const auto diffbot_urdf = +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD +======= + + + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1 1 @@ -407,16 +1429,29 @@ const auto diffbot_urdf = +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) +<<<<<<< HEAD +======= + + + + + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 1 1 @@ -450,6 +1485,7 @@ const auto diffbot_urdf = +<<<<<<< HEAD )"; @@ -457,6 +1493,669 @@ const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_unitilizable_robot_urdf = std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); +======= + + + test_hardware_components/TestIMUSensor + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_no_mimic = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_unknown_joint = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_incomplete = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_invalid_two_root_links = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_hardware_resources_no_command_if = + R"( + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_true_no_command_if = + R"( + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_true_command_if = + R"( + + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_false_command_if = + R"( + + + + + + + + + + + + + + )"; + +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 = + std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_urdf_with_different_hw_rw_rate = + std::string(urdf_head) + std::string(hardware_resources_with_different_rw_rates) + + std::string(urdf_tail); +const auto minimal_uninitializable_robot_urdf = + std::string(urdf_head) + std::string(uninitializable_hardware_resources) + std::string(urdf_tail); + +const auto minimal_robot_not_existing_actuator_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + + std::string(urdf_tail); +const auto minimal_robot_not_existing_sensors_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_sensor_plugin) + + std::string(urdf_tail); +const auto minimal_robot_not_existing_system_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_system_plugin) + + std::string(urdf_tail); + +const auto minimal_robot_duplicated_component = + std::string(urdf_head) + std::string(hardware_resources_duplicated_component) + + std::string(urdf_tail); + +const auto minimal_robot_actuator_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + + std::string(urdf_tail); +const auto minimal_robot_sensor_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_sensor_initializaion_error) + + std::string(urdf_tail); +const auto minimal_robot_system_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_system_initializaion_error) + + std::string(urdf_tail); +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + @@ -468,7 +2167,11 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_NAME = "TestActuatorHardware"; [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_TYPE = "actuator"; +<<<<<<< HEAD [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_CLASS_TYPE = "test_actuator"; +======= +[[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_PLUGIN_NAME = "test_actuator"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) [[maybe_unused]] const std::vector TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES = { "joint1/position", "joint1/max_velocity"}; [[maybe_unused]] const std::vector TEST_ACTUATOR_HARDWARE_STATE_INTERFACES = { @@ -476,14 +2179,22 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_NAME = "TestSensorHardware"; [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_TYPE = "sensor"; +<<<<<<< HEAD [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_CLASS_TYPE = "test_sensor"; +======= +[[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_PLUGIN_NAME = "test_sensor"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) [[maybe_unused]] const std::vector TEST_SENSOR_HARDWARE_COMMAND_INTERFACES = {""}; [[maybe_unused]] const std::vector TEST_SENSOR_HARDWARE_STATE_INTERFACES = { "sensor1/velocity"}; [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_NAME = "TestSystemHardware"; [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system"; +<<<<<<< HEAD [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_CLASS_TYPE = "test_system"; +======= +[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME = "test_system"; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = { "joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"}; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_STATE_INTERFACES = { diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp new file mode 100644 index 0000000000..eb2bbf24c7 --- /dev/null +++ b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023, FZI Forschungszentrum Informatik +// +// 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. +// +/// \author: Felix Exner + +#ifndef ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +namespace test_constants +{ +/// Constants defining special values used inside tests to trigger things like deactivate or errors +/// on read/write. +constexpr double READ_FAIL_VALUE = 28282828.0; +constexpr double WRITE_FAIL_VALUE = 23232323.0; +constexpr double READ_DEACTIVATE_VALUE = 29292929.0; +constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0; +} // namespace test_constants +#endif // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 4e1ec80d04..f4136a0bd8 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,11 @@ ros2_control_test_assets +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index a7cd431f5e..f3fc1a8167 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -159,6 +160,203 @@ Changelog for package ros2controlcli 2.15.0 (2022-09-19) ------------------- * pygraphviz functions updated (`#812 `_) (`#814 `_) +======= +4.21.0 (2024-12-06) +------------------- +* [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) +* Contributors: Sai Kishor Kothakota + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- +* [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_) +* Contributors: Sai Kishor Kothakota + +4.18.0 (2024-10-07) +------------------- +* [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) +* Contributors: Sai Kishor Kothakota + +4.17.0 (2024-09-11) +------------------- +* [ros2controlcli] fix list_controllers when no controllers are loaded (`#1721 `_) +* Contributors: Sai Kishor Kothakota + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- +* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) +* Make list controller and list hardware components immediately visualize the state. (`#1606 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + +4.13.0 (2024-07-08) +------------------- +* Remove ament linters (`#1601 `_) +* Contributors: Bence Magyar + +4.12.0 (2024-07-01) +------------------- + +4.11.0 (2024-05-14) +------------------- + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ +* [CI] Specify runner/container images and codecov for joint_limits (`#1504 `_) +* [CLI] Add `set_hardware_component_state` verb (`#1248 `_) +* Contributors: Christoph Fröhlich + +4.8.0 (2024-03-27) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-03-02) +------------------ +* Added spawner colours to `list_controllers` depending upon active or inactive (`#1409 `_) +* Contributors: Soham Patil + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ +* [docs] Remove joint_state_controller (`#1263 `_) +* Contributors: Christoph Fröhlich + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- +* Fix doc of load_controller (`#1132 `_) +* Contributors: Christoph Fröhlich + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- +* Add info where the pdf is saved to view_controller_chains (`#1094 `_) +* Contributors: Christoph Fröhlich + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- +* Improve list hardware components output and code for better readability. (`#1060 `_) +* Contributors: Dr. Denis + +3.14.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (`#1031 `_) +* Contributors: Christoph Fröhlich + +3.13.0 (2023-05-18) +------------------- +* Fix github links on control.ros.org (`#1019 `_) +* Contributors: Christoph Fröhlich + +3.12.2 (2023-04-29) +------------------- +* Fix verbose output of list_hardware_components (`#1004 `_) +* Contributors: Christoph Fröhlich + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ +* Remove deprecations from CLI and controller_manager (`#948 `_) +* [CLI] Fix wrong output of controller states for `load_controller` command (`#947 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-02-10) +------------------ +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Add list_hardware_components CLI `_ - Adds list_hardware_components to CLI (`#891 `_) +* Contributors: Andy McEvoy, Dr. Denis + +3.7.0 (2023-01-24) +------------------ +* Do not use CLI calls but direct API for setting parameters. (`#910 `_) +* Contributors: Dr. Denis + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* Fix hardware interface CLI description (`#864 `_) +* Contributors: Christoph Fröhlich + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ + +2.15.0 (2022-09-19) +------------------- +* migrate from graphviz python to pygraphviz (`#812 `_) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) * Contributors: Sachin Kumar 2.14.0 (2022-09-04) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index ce4924c67a..945747bd3e 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -28,6 +28,7 @@ list_controllers .. code-block:: console $ ros2 control list_controllers -h +<<<<<<< HEAD usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] Output the list of loaded controllers, their type and status @@ -40,6 +41,32 @@ list_controllers Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-s] [--claimed-interfaces] [--required-state-interfaces] [--required-command-interfaces] [--chained-interfaces] [--reference-interfaces] [--verbose] + [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + + Output the list of loaded controllers, their type and status + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --claimed-interfaces List controller's claimed interfaces + --required-state-interfaces + List controller's required state interfaces + --required-command-interfaces + List controller's required command interfaces + --chained-interfaces List interfaces that the controllers are chained to + --reference-interfaces + List controller's exported references + --verbose, -v List controller's claimed interfaces, required state interfaces and required command interfaces + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Example output: @@ -55,6 +82,7 @@ list_controller_types .. code-block:: console $ ros2 control list_controller_types -h +<<<<<<< HEAD usage: ros2 control list_controller_types [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] Output the available controller types and their base classes @@ -67,6 +95,22 @@ list_controller_types Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + usage: ros2 control list_controller_types [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + + Output the available controller types and their base classes + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Example output: @@ -84,11 +128,16 @@ list_hardware_components .. code-block:: console $ ros2 control list_hardware_components -h +<<<<<<< HEAD usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] +======= + usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [--verbose] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Output the list of available hardware components options: +<<<<<<< HEAD -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) @@ -99,6 +148,18 @@ list_hardware_components --include-hidden-nodes Consider hidden nodes as well +======= + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --verbose, -v List hardware components with command and state interfaces + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Example output: @@ -118,6 +179,7 @@ list_hardware_interfaces .. code-block:: console $ ros2 control list_hardware_interfaces -h +<<<<<<< HEAD usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] Output the list of available command and state interfaces @@ -130,6 +192,22 @@ list_hardware_interfaces Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + + Output the list of available command and state interfaces + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) .. code-block:: console @@ -149,26 +227,44 @@ load_controller .. code-block:: console $ ros2 control load_controller -h +<<<<<<< HEAD usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [-s] [--set-state {configured,inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name +======= + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [-s] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name [param_file] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Load a controller in a controller manager positional arguments: controller_name Name of the controller +<<<<<<< HEAD +======= + param_file The YAML file with the controller parameters +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time +<<<<<<< HEAD --set-state {configured,inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + --set-state {inactive,active} + Set the state of the loaded controller + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) reload_controller_libraries --------------------------- @@ -176,6 +272,7 @@ reload_controller_libraries .. code-block:: console $ ros2 control reload_controller_libraries -h +<<<<<<< HEAD usage: ros2 control reload_controller_libraries [-h] [--spin-time SPIN_TIME] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] Reload controller libraries @@ -189,6 +286,23 @@ reload_controller_libraries Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + usage: ros2 control reload_controller_libraries [-h] [--spin-time SPIN_TIME] [-s] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + + Reload controller libraries + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --force-kill Force stop of loaded controllers + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) set_controller_state -------------------- @@ -196,12 +310,17 @@ set_controller_state .. code-block:: console $ ros2 control set_controller_state -h +<<<<<<< HEAD usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name {inactive,active} +======= + usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name {inactive,active} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Adjust the state of the controller positional arguments: controller_name Name of the controller to be changed +<<<<<<< HEAD {inactive,active} State in which the controller should be changed to @@ -213,6 +332,20 @@ set_controller_state Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + {inactive,active} State in which the controller should be changed to + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) set_hardware_component_state ---------------------------- @@ -220,8 +353,12 @@ set_hardware_component_state .. code-block:: console $ ros2 control set_hardware_component_state -h +<<<<<<< HEAD usage: ros2 control set_hardware_component_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] hardware_component_name {unconfigured,inactive,active} +======= + usage: ros2 control set_hardware_component_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] hardware_component_name {unconfigured,inactive,active} +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Adjust the state of the hardware component @@ -237,9 +374,16 @@ set_hardware_component_state Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER +<<<<<<< HEAD Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) switch_controllers ------------------ @@ -247,6 +391,7 @@ switch_controllers .. code-block:: console $ ros2 control switch_controllers -h +<<<<<<< HEAD usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [--deactivate [CTRL1 [CTRL2 ...]]] [--activate [CTRL1 [CTRL2 ...]]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] @@ -268,6 +413,31 @@ switch_controllers Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [-s] [--deactivate [DEACTIVATE ...]] [--activate [ACTIVATE ...]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] + [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + + Switch controllers in a controller manager + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --deactivate [DEACTIVATE ...] + Name of the controllers to be deactivated + --activate [ACTIVATE ...] + Name of the controllers to be activated + --strict Strict switch + --activate-asap Start asap controllers + --switch-timeout SWITCH_TIMEOUT + Timeout for switching controllers + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) unload_controller ----------------- @@ -275,13 +445,18 @@ unload_controller .. code-block:: console $ ros2 control unload_controller -h +<<<<<<< HEAD usage: ros2 control unload_controller [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name +======= + usage: ros2 control unload_controller [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Unload a controller in a controller manager positional arguments: controller_name Name of the controller +<<<<<<< HEAD optional arguments: -h, --help show this help message and exit --spin-time SPIN_TIME @@ -290,6 +465,18 @@ unload_controller Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) view_controller_chains ---------------------- @@ -297,7 +484,11 @@ view_controller_chains .. code-block:: console $ ros2 control view_controller_chains -h +<<<<<<< HEAD usage: ros2 view_controller_chains +======= + usage: ros2 control view_controller_chains [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Generates a diagram of the loaded chained controllers into /tmp/controller_diagram.gv.pdf @@ -307,6 +498,13 @@ view_controller_chains Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER +<<<<<<< HEAD Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well +======= + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 0b9164757f..073f1f9a3e 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,11 @@ ros2controlcli +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) The ROS 2 command line tools for ROS2 Control. @@ -21,11 +25,14 @@ rosidl_runtime_py python3-pygraphviz +<<<<<<< HEAD ament_copyright ament_flake8 ament_pep257 ament_xmllint +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_python diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 2b4b805980..6b8e02c4a7 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -15,14 +15,18 @@ from controller_manager import list_controllers, list_hardware_components +<<<<<<< HEAD import rclpy +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) from ros2cli.node.direct import DirectNode from ros2node.api import NodeNameCompleter from ros2param.api import call_list_parameters +<<<<<<< HEAD def service_caller(service_name, service_type, request): try: @@ -49,6 +53,9 @@ def service_caller(service_name, service_type, request): finally: node.destroy_node() rclpy.shutdown() +======= +import argparse +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class ControllerNameCompleter: @@ -89,6 +96,7 @@ def __call__(self, prefix, parsed_args, **kwargs): return [c.name for c in hardware_components if c.state.label in self.valid_states] +<<<<<<< HEAD def add_controller_mgr_parsers(parser): """Parser arguments to get controller manager node name, defaults to /controller_manager.""" arg = parser.add_argument( @@ -96,9 +104,33 @@ def add_controller_mgr_parsers(parser): "--controller-manager", help="Name of the controller manager ROS node", default="/controller_manager", +======= +class ParserROSArgs(argparse.Action): + def __call__(self, parser, namespace, values, option_string=None): + values = [option_string] + values + setattr(namespace, "argv", values) + + +def add_controller_mgr_parsers(parser): + """Parser arguments to get controller manager node name, defaults to controller_manager.""" + arg = parser.add_argument( + "-c", + "--controller-manager", + help="Name of the controller manager ROS node (default: controller_manager)", + default="controller_manager", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) required=False, ) arg.completer = NodeNameCompleter(include_hidden_nodes_key="include_hidden_nodes") parser.add_argument( "--include-hidden-nodes", action="store_true", help="Consider hidden nodes as well" ) +<<<<<<< HEAD +======= + parser.add_argument( + "--ros-args", + nargs=argparse.REMAINDER, + help="Pass arbitrary arguments to the executable", + action=ParserROSArgs, + ) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/ros2controlcli/ros2controlcli/verb/list_controller_types.py b/ros2controlcli/ros2controlcli/verb/list_controller_types.py index 086b820124..7c96e2905c 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controller_types.py +++ b/ros2controlcli/ros2controlcli/verb/list_controller_types.py @@ -29,7 +29,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) response = list_controller_types(node, args.controller_manager) types_and_classes = zip(response.types, response.base_classes) for c in types_and_classes: diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index b4ebff94cd..51855c14df 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,6 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +<<<<<<< HEAD +======= +import warnings +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments @@ -50,10 +54,21 @@ def print_controller_state(c, args, col_width_name, col_width_state, col_width_t for connection in c.chain_connections: for reference in connection.reference_interfaces: print(f"\t\t{reference:20s}") +<<<<<<< HEAD if args.reference_interfaces or args.verbose: print("\texported reference interfaces:") for reference_interfaces in c.reference_interfaces: print(f"\t\t{reference_interfaces}") +======= + if args.reference_interfaces or args.exported_interfaces or args.verbose: + print("\texported reference interfaces:") + for reference_interface in c.reference_interfaces: + print(f"\t\t{reference_interface}") + if args.reference_interfaces or args.exported_interfaces or args.verbose: + print("\texported state interfaces:") + for exported_state_interface in c.exported_state_interfaces: + print(f"\t\t{exported_state_interface}") +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class ListControllersVerb(VerbExtension): @@ -87,6 +102,14 @@ def add_arguments(self, parser, cli_name): help="List controller's exported references", ) parser.add_argument( +<<<<<<< HEAD +======= + "--exported-interfaces", + action="store_true", + help="List controller's exported state and reference interfaces", + ) + parser.add_argument( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "--verbose", "-v", action="store_true", @@ -95,9 +118,22 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) +======= + with NodeStrategy(args).direct_node as node: + response = list_controllers(node, args.controller_manager) + + if args.reference_interfaces: + warnings.filterwarnings("always") + warnings.warn( + "The '--reference-interfaces' argument is deprecated and will be removed in future releases. Use '--exported-interfaces' instead.", + DeprecationWarning, + ) + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if not response.controller: print("No controllers are currently loaded!") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index d614cbc7ea..3e79c5f23c 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -36,7 +36,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): @@ -54,9 +58,12 @@ def main(self, *, args): ) if hasattr(component, "plugin_name"): plugin_name = f"{component.plugin_name}" +<<<<<<< HEAD # Keep compatibility to the obsolete filed name in Humble elif hasattr(component, "class_type"): plugin_name = f"{component.class_type}" +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) else: plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" @@ -66,7 +73,10 @@ def main(self, *, args): f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if cmd_interface.is_available: available_str = f"{bcolors.OKBLUE}[available]{bcolors.ENDC}" else: diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 4510998ad9..079de59f30 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -28,7 +28,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) hardware_interfaces = list_hardware_interfaces(node, args.controller_manager) command_interfaces = sorted( hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index f28c9c1eb2..d401a05876 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -12,13 +12,29 @@ # See the License for the specific language governing permissions and # limitations under the License. +<<<<<<< HEAD from controller_manager import configure_controller, load_controller, switch_controllers +======= +from controller_manager import ( + configure_controller, + load_controller, + list_controllers, + switch_controllers, + set_controller_parameters_from_param_files, + bcolors, +) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2cli.verb import VerbExtension from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter +<<<<<<< HEAD +======= +import os +from argparse import OPTIONAL +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) class LoadControllerVerb(VerbExtension): @@ -29,13 +45,27 @@ def add_arguments(self, parser, cli_name): arg = parser.add_argument("controller_name", help="Name of the controller") arg.completer = ControllerNameCompleter() arg = parser.add_argument( +<<<<<<< HEAD "--set-state", choices=["configured", "inactive", "active"], help="Set the state of the loaded controller", +======= + "param_file", + help="The YAML file with the controller parameters", + nargs=OPTIONAL, + default=None, + ) + arg = parser.add_argument( + "--set-state", + choices=["inactive", "active"], + help="Set the state of the loaded controller", + default=None, +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ) add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: response = load_controller(node, args.controller_manager, args.controller_name) if not response.ok: @@ -67,3 +97,66 @@ def main(self, *, args): f'state {"inactive" if args.set_state == "inactive" else "active"}' ) return 0 +======= + with NodeStrategy(args).direct_node as node: + controllers = list_controllers(node, args.controller_manager, 20.0).controller + if any(c.name == args.controller_name for c in controllers): + print( + f"{bcolors.WARNING}Controller : {args.controller_name} already loaded, skipping load_controller!{bcolors.ENDC}" + ) + else: + if args.param_file: + if not os.path.exists(args.param_file): + print( + f"{bcolors.FAIL}Controller parameter file : {args.param_file} does not exist, Aborting!{bcolors.ENDC}" + ) + return 1 + if not os.path.isabs(args.param_file): + args.param_file = os.path.join(os.getcwd(), args.param_file) + + if not set_controller_parameters_from_param_files( + node, + args.controller_manager, + args.controller_name, + [args.param_file], + node.get_namespace(), + ): + return 1 + + ret = load_controller(node, args.controller_manager, args.controller_name) + if not ret.ok: + print( + f"{bcolors.FAIL}Failed loading controller {args.controller_name} check controller_manager logs{bcolors.ENDC}" + ) + return 1 + print( + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name}{bcolors.ENDC}" + ) + + if args.set_state: + + # we in any case configure the controller + response = configure_controller( + node, args.controller_manager, args.controller_name + ) + if not response.ok: + print( + f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}" + ) + return 1 + + if args.set_state == "active": + response = switch_controllers( + node, args.controller_manager, [], [args.controller_name], True, True, 5.0 + ) + if not response.ok: + print( + f"{bcolors.FAIL}Error activating controller : {args.controller_name}, check controller_manager logs{bcolors.ENDC}" + ) + return 1 + + print( + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name} into state {args.set_state}{bcolors.ENDC}" + ) + return 0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py index 82bc2e480f..de5d1ca339 100644 --- a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py +++ b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py @@ -32,7 +32,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) response = reload_controller_libraries( node, args.controller_manager, force_kill=args.force_kill ) diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index 9bd7c5f01e..ba19436090 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -37,7 +37,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) controllers = list_controllers(node, args.controller_manager).controller try: @@ -58,6 +62,7 @@ def main(self, *, args): # # print(f'successfully cleaned up {args.controller_name}') # return 0 +<<<<<<< HEAD if args.state == "configure": args.state = "inactive" print('Setting state "configure" is deprecated, use "inactive" instead!') @@ -66,6 +71,8 @@ def main(self, *, args): args.state = "inactive" print('Setting state "stop" is deprecated, use "inactive" instead!') +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if args.state == "inactive": if matched_controller.state == "unconfigured": response = configure_controller( @@ -93,10 +100,13 @@ def main(self, *, args): f"from its current state {matched_controller.state}" ) +<<<<<<< HEAD if args.state == "start": args.state = "active" print('Setting state "start" is deprecated, use "active" instead!') +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if args.state == "active": if matched_controller.state != "inactive": return ( diff --git a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py index 4b1093f4f7..17140abd13 100644 --- a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -39,7 +39,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if args.state == "unconfigured": diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 141fefd593..95f9a6fc5a 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -12,7 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +<<<<<<< HEAD from controller_manager import switch_controllers +======= +from controller_manager import switch_controllers, bcolors +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -27,6 +31,7 @@ class SwitchControllersVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) arg = parser.add_argument( +<<<<<<< HEAD "--stop", nargs="*", default=[], @@ -34,6 +39,8 @@ def add_arguments(self, parser, cli_name): ) arg.completer = LoadedControllerNameCompleter(["active"]) arg = parser.add_argument( +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "--deactivate", nargs="*", default=[], @@ -41,6 +48,7 @@ def add_arguments(self, parser, cli_name): ) arg.completer = LoadedControllerNameCompleter(["active"]) arg = parser.add_argument( +<<<<<<< HEAD "--start", nargs="*", default=[], @@ -48,6 +56,8 @@ def add_arguments(self, parser, cli_name): ) arg.completer = LoadedControllerNameCompleter(["inactive"]) arg = parser.add_argument( +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) "--activate", nargs="*", default=[], @@ -55,7 +65,10 @@ def add_arguments(self, parser, cli_name): ) arg.completer = LoadedControllerNameCompleter(["inactive"]) parser.add_argument("--strict", action="store_true", help="Strict switch") +<<<<<<< HEAD parser.add_argument("--start-asap", action="store_true", help="Start asap controllers") +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) parser.add_argument("--activate-asap", action="store_true", help="Start asap controllers") parser.add_argument( "--switch-timeout", @@ -67,6 +80,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD if args.stop: print('"--stop" flag is deprecated, use "--deactivate" instead!') args.deactivate = args.stop @@ -78,12 +92,16 @@ def main(self, *, args): args.activate_asap = args.start_asap with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) response = switch_controllers( node, args.controller_manager, args.deactivate, args.activate, args.strict, +<<<<<<< HEAD args.start_asap, args.switch_timeout, ) @@ -91,4 +109,18 @@ def main(self, *, args): return "Error switching controllers, check controller_manager logs" print("Successfully switched controllers") +======= + args.activate_asap, + args.switch_timeout, + ) + if not response.ok: + print( + bcolors.FAIL + + "Error switching controllers, check controller_manager logs" + + bcolors.ENDC + ) + return 1 + + print(bcolors.OKBLUE + "Successfully switched controllers" + bcolors.ENDC) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/unload_controller.py b/ros2controlcli/ros2controlcli/verb/unload_controller.py index 81612eb3ad..be3077bede 100644 --- a/ros2controlcli/ros2controlcli/verb/unload_controller.py +++ b/ros2controlcli/ros2controlcli/verb/unload_controller.py @@ -12,7 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +<<<<<<< HEAD from controller_manager import unload_controller +======= +from controller_manager import unload_controller, bcolors +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -31,10 +35,24 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: response = unload_controller(node, args.controller_manager, args.controller_name) if not response.ok: return "Error unloading controllers, check controller_manager logs" print(f"Successfully unloaded controller {args.controller_name}") +======= + with NodeStrategy(args).direct_node as node: + response = unload_controller(node, args.controller_manager, args.controller_name) + if not response.ok: + print( + f"{bcolors.FAIL}Error unloading controller {args.controller_name}, check controller_manager logs{bcolors.ENDC}" + ) + return 1 + + print( + f"{bcolors.OKBLUE}Successfully unloaded controller {args.controller_name}{bcolors.ENDC}" + ) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py index f838a96e1c..e280b4c58f 100644 --- a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py +++ b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py @@ -202,7 +202,11 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): +<<<<<<< HEAD with NodeStrategy(args) as node: +======= + with NodeStrategy(args).direct_node as node: +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) list_controllers_response = list_controllers(node, args.controller_manager) list_hardware_response = list_hardware_interfaces(node, args.controller_manager) parse_response(list_controllers_response, list_hardware_response) diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index ee94392cc8..4d963a2273 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,11 @@ setup( name=package_name, +<<<<<<< HEAD version="2.45.0", +======= + version="4.21.0", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/ros2controlcli/test/test_view_controller_chains.py b/ros2controlcli/test/test_view_controller_chains.py index 62e1bf5ac7..d7a07f47b4 100644 --- a/ros2controlcli/test/test_view_controller_chains.py +++ b/ros2controlcli/test/test_view_controller_chains.py @@ -23,9 +23,13 @@ class TestViewControllerChains(unittest.TestCase): +<<<<<<< HEAD def test_expected(self): +======= + def test_expected(self): +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) list_controllers_response = ListControllers.Response() list_hardware_response = ListHardwareInterfaces.Response() diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 06b61e52e7..9a35e45d6c 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -133,6 +134,177 @@ Changelog for package rqt_controller_manager 2.15.0 (2022-09-19) ------------------- +======= +4.21.0 (2024-12-06) +------------------- + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- +* fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_) +* Contributors: Santosh Govindaraj + +4.18.0 (2024-10-07) +------------------- + +4.17.0 (2024-09-11) +------------------- + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + +4.13.0 (2024-07-08) +------------------- + +4.12.0 (2024-07-01) +------------------- +* [rqt_controller_manager] Add hardware components (`#1455 `_) +* Contributors: Christoph Fröhlich + +4.11.0 (2024-05-14) +------------------- + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ + +4.8.0 (2024-03-27) +------------------ +* Fix rqt_controller_manager for non-humble (`#1454 `_) +* Add cm as dependency to rqt_cm (`#1447 `_) +* Contributors: Christoph Fröhlich + +4.7.0 (2024-03-22) +------------------ +* Codeformat from new pre-commit config (`#1433 `_) +* rqt_controller_manager compatibility for humble (`#1429 `_) +* Contributors: Christoph Fröhlich + +4.6.0 (2024-03-02) +------------------ +* [CI] Code coverage + pre-commit (`#1413 `_) +* Contributors: Christoph Fröhlich + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ +* Fix rqt controller manager crash on ros2_control restart (`#1273 `_) +* Contributors: Sai Kishor Kothakota + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Contributors: Dr. Denis + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ +* 🔧 Fixes and updated on pre-commit hooks and their action (`#890 `_) +* Contributors: Denis Štogl + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ +* rqt controller manager ros 2 port (`#813 `_) +* Contributors: Kenji Brameld + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.14.0 (2022-09-04) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index a898162a7e..20c1820c04 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,11 @@ rqt_controller_manager +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 0044649b6f..28f41b9ba0 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -173,7 +173,10 @@ def _on_cm_change(self, cm_name): self._update_hw_components() def _update_controllers(self): +<<<<<<< HEAD +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) if not self._cm_name: return @@ -634,4 +637,15 @@ def _get_parameter_controller_names(node, node_name): """Get list of ROS parameter names that potentially represent a controller configuration.""" parameter_names = call_list_parameters(node=node, node_name=node_name) suffix = ".type" +<<<<<<< HEAD return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] +======= + # @note: The versions conditioning is added here to support the source-compatibility with Humble + if os.environ.get("ROS_DISTRO") == "humble": + # for humble, ros2param < 0.20.0 + return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] + else: + return [ + n[: -len(suffix)] for n in parameter_names.result().result.names if n.endswith(suffix) + ] +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index a3d40472c7..7820ae0ce0 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,11 @@ setup( name=package_name, +<<<<<<< HEAD version="2.45.0", +======= + version="4.21.0", +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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 be80039f40..c1d85ef520 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +<<<<<<< HEAD 2.45.0 (2024-12-03) ------------------- @@ -130,6 +131,184 @@ Changelog for package transmission_interface 2.16.0 (2022-10-17) ------------------- +======= +4.21.0 (2024-12-06) +------------------- + +4.20.0 (2024-11-08) +------------------- + +4.19.0 (2024-10-26) +------------------- + +4.18.0 (2024-10-07) +------------------- + +4.17.0 (2024-09-11) +------------------- + +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- +* Preparation of Handles for Variant Support (`#1678 `_) +* Fix flaky transmission_interface tests by making them deterministic. (`#1665 `_) +* Contributors: Manuel Muth, sgmurray + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + +4.13.0 (2024-07-08) +------------------- + +4.12.0 (2024-07-01) +------------------- + +4.11.0 (2024-05-14) +------------------- +* Add find_package for ament_cmake_gen_version_h (`#1534 `_) +* Contributors: Christoph Fröhlich + +4.10.0 (2024-05-08) +------------------- + +4.9.0 (2024-04-30) +------------------ +* rosdoc2 for transmission_interface (`#1496 `_) +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + +4.8.0 (2024-03-27) +------------------ +* generate version.h file per package using the ament_generate_version_header (`#1449 `_) +* Contributors: Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-03-02) +------------------ +* Add -Werror=missing-braces to compile options (`#1423 `_) +* Contributors: Sai Kishor Kothakota + +4.5.0 (2024-02-12) +------------------ + +4.4.0 (2024-01-31) +------------------ + +4.3.0 (2024-01-20) +------------------ +* Improve transmission tests (`#1238 `_) +* Contributors: Maximilian Schik + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ + +3.21.0 (2023-11-06) +------------------- + +3.20.0 (2023-10-31) +------------------- + +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + +3.16.0 (2023-07-09) +------------------- + +3.15.0 (2023-06-23) +------------------- + +3.14.0 (2023-06-14) +------------------- +* Add -Wconversion flag to protect future developments (`#1053 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#1037 `_) +* Contributors: Sai Kishor Kothakota, gwalck + +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + +3.12.1 (2023-04-14) +------------------- + +3.12.0 (2023-04-02) +------------------- + +3.11.0 (2023-03-22) +------------------- + +3.10.0 (2023-03-16) +------------------- +* Split transmission interfaces (`#938 `_) +* Contributors: Noel Jiménez García + +3.9.1 (2023-03-09) +------------------ +* Fix missing include (`#963 `_) +* Contributors: Bence Magyar + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 2.15.0 (2022-09-19) ------------------- diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index f7dd4444b3..28947ac1f7 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -1,3 +1,4 @@ +<<<<<<< HEAD cmake_minimum_required(VERSION 3.5) project(transmission_interface) @@ -15,20 +16,43 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +======= +cmake_minimum_required(VERSION 3.16) +project(transmission_interface LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Werror=missing-braces) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp +) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_gen_version_h REQUIRED) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +<<<<<<< HEAD install( DIRECTORY include/ DESTINATION include ) add_library(${PROJECT_NAME} SHARED +======= +add_library(transmission_interface SHARED +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) src/simple_transmission_loader.cpp src/four_bar_linkage_transmission_loader.cpp src/differential_transmission_loader.cpp ) +<<<<<<< HEAD target_include_directories(${PROJECT_NAME} PUBLIC include) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -85,6 +109,51 @@ if(BUILD_TESTING) ) target_include_directories(test_differential_transmission_loader PUBLIC include hardware_interface) ament_target_dependencies(test_differential_transmission_loader hardware_interface) +======= +target_include_directories(transmission_interface PUBLIC + $ + $ +) +ament_target_dependencies(transmission_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +pluginlib_export_plugin_description_file(transmission_interface ros2_control_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_simple_transmission + test/simple_transmission_test.cpp + ) + target_link_libraries(test_simple_transmission transmission_interface) + + ament_add_gmock(test_differential_transmission + test/differential_transmission_test.cpp + ) + target_link_libraries(test_differential_transmission transmission_interface) + + ament_add_gmock(test_four_bar_linkage_transmission + test/four_bar_linkage_transmission_test.cpp + ) + target_link_libraries(test_four_bar_linkage_transmission transmission_interface) + + ament_add_gmock(test_simple_transmission_loader + test/simple_transmission_loader_test.cpp + ) + target_link_libraries(test_simple_transmission_loader transmission_interface) + ament_target_dependencies(test_simple_transmission_loader ros2_control_test_assets) + + ament_add_gmock(test_four_bar_linkage_transmission_loader + test/four_bar_linkage_transmission_loader_test.cpp + ) + target_link_libraries(test_four_bar_linkage_transmission_loader transmission_interface) + ament_target_dependencies(test_four_bar_linkage_transmission_loader ros2_control_test_assets) + + ament_add_gmock(test_differential_transmission_loader + test/differential_transmission_loader_test.cpp + ) + target_link_libraries(test_differential_transmission_loader transmission_interface) + ament_target_dependencies(test_differential_transmission_loader ros2_control_test_assets) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_add_gmock( test_utils @@ -94,6 +163,7 @@ if(BUILD_TESTING) ament_target_dependencies(test_utils hardware_interface) endif() +<<<<<<< HEAD ament_export_include_directories( include ) @@ -105,5 +175,20 @@ ament_export_libraries( ${PROJECT_NAME} ) +======= +install( + DIRECTORY include/ + DESTINATION include/transmission_interface +) +install(TARGETS transmission_interface + EXPORT export_transmission_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_transmission_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_package() ament_generate_version_header(${PROJECT_NAME}) diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index dd58e55744..6b8be65592 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -17,6 +17,10 @@ #include #include +<<<<<<< HEAD +======= +#include +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 85da01116d..ef3c7b214e 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -16,7 +16,10 @@ #define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_ #include +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 863c20da9b..8df6796808 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -21,6 +21,10 @@ #include #include +<<<<<<< HEAD +======= +#include "hardware_interface/types/hardware_interface_type_values.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/accessor.hpp" #include "transmission_interface/exception.hpp" #include "transmission_interface/transmission.hpp" diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..ea3359475e 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -15,13 +15,17 @@ #ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_ #define TRANSMISSION_INTERFACE__HANDLE_HPP_ +<<<<<<< HEAD #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "hardware_interface/handle.hpp" namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ +<<<<<<< HEAD class ActuatorHandle : public hardware_interface::ReadWriteHandle { public: @@ -33,6 +37,19 @@ class JointHandle : public hardware_interface::ReadWriteHandle { public: using hardware_interface::ReadWriteHandle::ReadWriteHandle; +======= +class ActuatorHandle : public hardware_interface::Handle +{ +public: + using hardware_interface::Handle::Handle; +}; + +/** A handle used to get and set a value on a given joint interface. */ +class JointHandle : public hardware_interface::Handle +{ +public: + using hardware_interface::Handle::Handle; +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) }; } // namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 697e1a4eb7..e87e6d3293 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -16,8 +16,11 @@ #include #include +<<<<<<< HEAD #include #include +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include #include "transmission_interface/handle.hpp" diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 673d4868a9..c27f489432 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,11 @@ transmission_interface +<<<<<<< HEAD 2.45.0 +======= + 4.21.0 +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 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 @@ -15,6 +19,10 @@ pluginlib ament_cmake_gmock +<<<<<<< HEAD +======= + ros2_control_test_assets +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ament_cmake diff --git a/transmission_interface/src/differential_transmission_loader.cpp b/transmission_interface/src/differential_transmission_loader.cpp index 37f5b708c7..dc70885a21 100644 --- a/transmission_interface/src/differential_transmission_loader.cpp +++ b/transmission_interface/src/differential_transmission_loader.cpp @@ -16,12 +16,18 @@ #include +<<<<<<< HEAD #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/rclcpp.hpp" +======= +#include "hardware_interface/hardware_info.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/differential_transmission.hpp" namespace transmission_interface diff --git a/transmission_interface/src/four_bar_linkage_transmission_loader.cpp b/transmission_interface/src/four_bar_linkage_transmission_loader.cpp index 93b42e33bf..d8a146e707 100644 --- a/transmission_interface/src/four_bar_linkage_transmission_loader.cpp +++ b/transmission_interface/src/four_bar_linkage_transmission_loader.cpp @@ -16,12 +16,18 @@ #include +<<<<<<< HEAD #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/rclcpp.hpp" +======= +#include "hardware_interface/hardware_info.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/four_bar_linkage_transmission.hpp" namespace transmission_interface diff --git a/transmission_interface/src/simple_transmission_loader.cpp b/transmission_interface/src/simple_transmission_loader.cpp index aaa86bff22..bf81abbc51 100644 --- a/transmission_interface/src/simple_transmission_loader.cpp +++ b/transmission_interface/src/simple_transmission_loader.cpp @@ -16,12 +16,18 @@ #include +<<<<<<< HEAD #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/rclcpp.hpp" +======= +#include "hardware_interface/hardware_info.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/logging.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/simple_transmission.hpp" namespace transmission_interface diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index 70d0adf2cd..4a0c2d0290 100644 --- a/transmission_interface/test/differential_transmission_loader_test.cpp +++ b/transmission_interface/test/differential_transmission_loader_test.cpp @@ -20,10 +20,16 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" #include "transmission_interface/differential_transmission.hpp" #include "transmission_interface/differential_transmission_loader.hpp" +======= +#include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "transmission_interface/differential_transmission.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/transmission_loader.hpp" using testing::DoubleNear; @@ -57,9 +63,13 @@ class TransmissionPluginLoader TEST(DifferentialTransmissionLoaderTest, FullSpec) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -132,9 +142,13 @@ TEST(DifferentialTransmissionLoaderTest, FullSpec) TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -203,9 +217,13 @@ TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -266,9 +284,13 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -338,9 +360,13 @@ TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -413,9 +439,13 @@ TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) TEST(DifferentialTransmissionLoaderTest, mech_red_invalid_value) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp index 53b584b7b5..60634450f1 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -20,10 +20,16 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" #include "transmission_interface/four_bar_linkage_transmission.hpp" #include "transmission_interface/four_bar_linkage_transmission_loader.hpp" +======= +#include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "transmission_interface/four_bar_linkage_transmission.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/transmission_loader.hpp" using testing::DoubleNear; @@ -57,9 +63,13 @@ class TransmissionPluginLoader TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -133,9 +143,13 @@ TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -205,9 +219,13 @@ TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -269,9 +287,13 @@ TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -342,9 +364,13 @@ TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -418,9 +444,13 @@ TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index f74e4def6a..2dc5b827ed 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -26,7 +26,10 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using testing::DoubleNear; +<<<<<<< HEAD using testing::Not; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using transmission_interface::ActuatorHandle; using transmission_interface::Exception; using transmission_interface::FourBarLinkageTransmission; diff --git a/transmission_interface/test/simple_transmission_loader_test.cpp b/transmission_interface/test/simple_transmission_loader_test.cpp index 4623d8c409..c8793195ae 100644 --- a/transmission_interface/test/simple_transmission_loader_test.cpp +++ b/transmission_interface/test/simple_transmission_loader_test.cpp @@ -20,10 +20,16 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +<<<<<<< HEAD #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" #include "transmission_interface/simple_transmission.hpp" #include "transmission_interface/simple_transmission_loader.hpp" +======= +#include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "transmission_interface/simple_transmission.hpp" +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) #include "transmission_interface/transmission_loader.hpp" using testing::DoubleNear; @@ -58,6 +64,7 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) { // Parse transmission info +<<<<<<< HEAD std::string urdf_to_test = R"( @@ -158,6 +165,10 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) ros2_control_demo_hardware/VelocityActuatorHardware @@ -239,9 +250,13 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) { +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -285,9 +300,13 @@ TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) { +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -327,9 +346,13 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) { +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -369,9 +392,13 @@ TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(SimpleTransmissionLoaderTest, offset_ill_defined) { +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) @@ -413,11 +440,17 @@ TEST(SimpleTransmissionLoaderTest, offset_ill_defined) TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) { +<<<<<<< HEAD std::string urdf_to_test = R"( +======= + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( + + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) -1 1 @@ -426,7 +459,11 @@ TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) transmission_interface/SimpleTransmission +<<<<<<< HEAD +======= + +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) 0 diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 33a14f92d1..5e14f869e7 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -21,7 +21,10 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +<<<<<<< HEAD using std::vector; +======= +>>>>>>> bb087e2 (Fix the spawner to support full wildcard parameter entries (#1933)) using transmission_interface::ActuatorHandle; using transmission_interface::Exception; using transmission_interface::JointHandle;