From 437d37b6b395d0fa29c35bb50716103275f80dae Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Thu, 13 Apr 2023 23:47:03 -0700 Subject: [PATCH 01/50] Add missing build_export_depends to controller_interface (#989) --- controller_interface/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 9d2109f975..37a621359a 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -14,6 +14,9 @@ rclcpp_lifecycle sensor_msgs + hardware_interface + rclcpp_lifecycle + ament_cmake_gmock sensor_msgs From f33c1458d193ea6a6712640d6b11ce19eb8677da Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 14 Apr 2023 18:37:25 +0100 Subject: [PATCH 02/50] Update changelogs --- controller_interface/CHANGELOG.rst | 5 +++++ controller_manager/CHANGELOG.rst | 3 +++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 32 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 1ece36fa89..cf26f3c61a 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index f095c301a7..0f4ea49c51 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- * [Controller Interface] Add time and period paramters to update_reference_from_subscribers() (`#846 `_) #API-break diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index dbf79ad8a9..651f163411 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index fa0ea0ae19..d0d177bc36 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index a7cf9c387b..eaae1f10fb 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- * Extend joint limits structure with deceleration limits. (`#977 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index fd462592e8..c2fd1b75b7 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 6e2c484456..669dbffef0 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 9bd7966278..60331747f4 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index d3f015c111..0f630e09d9 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index be1200c9a5..73dbe472f7 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-04-02) ------------------- From 2484c7bc3c187984b5461a64ad9734fd5e936ffa Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 14 Apr 2023 18:37:54 +0100 Subject: [PATCH 03/50] 3.12.1 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index cf26f3c61a..a81a293ff3 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- * Add missing build_export_depends to controller_interface (`#989 `_) * Contributors: Scott K Logan diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 37a621359a..510c53d057 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.12.0 + 3.12.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 0f4ea49c51..8372b0db3f 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/controller_manager/package.xml b/controller_manager/package.xml index f9ea79c9c6..6f521cedc8 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.12.0 + 3.12.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 651f163411..5411ab39b5 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 021fcb41cf..ae70020730 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.12.0 + 3.12.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index d0d177bc36..b819b8921b 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 0d3ffdbe04..b257841b68 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.12.0 + 3.12.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index eaae1f10fb..9180ad3295 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index d517c8c0ce..081478e904 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.12.0 + 3.12.1 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index c2fd1b75b7..2e1c00c372 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index dda2bdfea2..601ed4bdc8 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.12.0 + 3.12.1 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 669dbffef0..892186bd89 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index f654a6efa1..568ce07a65 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.12.0 + 3.12.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 60331747f4..73687b1dd2 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 21d571f66a..24773e9992 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.12.0 + 3.12.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 86db51595e..9455ffc0d7 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.12.0", + version="3.12.1", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 0f630e09d9..ab6544a27f 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index c980235c03..07571ab6e2 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.12.0 + 3.12.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 55c6a3133f..7e9127b209 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.12.0", + version="3.12.1", 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 73dbe472f7..6ec67d7349 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.1 (2023-04-14) +------------------- 3.12.0 (2023-04-02) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 6b611b881c..8297097348 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.12.0 + 3.12.1 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 02b2bbf843b599916f230da0b909372525fd5acb Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 21 Apr 2023 15:08:14 +0200 Subject: [PATCH 04/50] Bump codecov/codecov-action from 3.1.1 to 3.1.2 (#992) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.1 to 3.1.2. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.1...v3.1.2) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 31abce9855..62ccd56b0c 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -44,7 +44,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.1 + - uses: codecov/codecov-action@v3.1.2 with: file: ros_ws/lcov/total_coverage.info flags: unittests From d753e5fa7696b79572ef29673f36eeeca8a6e75a Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 27 Apr 2023 21:15:35 +0200 Subject: [PATCH 05/50] Bump actions/setup-python from 4.5.0 to 4.6.0 (#996) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4.5.0 to 4.6.0. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4.5.0...v4.6.0) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 3e6def8be3..2020dd2f77 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.5.0 + - uses: actions/setup-python@v4.6.0 with: python-version: '3.10' - name: Install system hooks From 476449933838ab0c4464702d11bd7c5830743e17 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 27 Apr 2023 21:16:21 +0200 Subject: [PATCH 06/50] Bump codecov/codecov-action from 3.1.2 to 3.1.3 (#997) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.2 to 3.1.3. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.2...v3.1.3) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 62ccd56b0c..5ca0d46b1e 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -44,7 +44,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.2 + - uses: codecov/codecov-action@v3.1.3 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 3a3081b786a4676b5ab3493a48088246876d6a68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 29 Apr 2023 11:00:30 +0200 Subject: [PATCH 07/50] Fix docs format (#1002) --- doc/index.rst | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 9d614e1910..5821b795f3 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,23 +1,27 @@ .. _ros2_control_framework: +################# +ros2_control +################# + +`GitHub Repository `_ + ================= API Documentation ================= API documentation is parsed by doxygen and can be found `here <../../api/index.html>`_ -===================== -Core functionalities -===================== -`GitHub Repository `_ +========= Features ========= -- :ref:`Command Line Interface (CLI) ` +* :ref:`Command Line Interface (CLI) ` +======== Concepts ======== From e376c0bc87f8d3c8af079ee2d934c6dded22a06e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 29 Apr 2023 12:31:05 +0200 Subject: [PATCH 08/50] Fix verbose output of list_hardware_components (#1004) --- .../ros2controlcli/verb/list_hardware_components.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 2f94d70969..54dff21bb6 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -65,17 +65,12 @@ def main(self, *, args): if args.verbose: print("\tstate interfaces") - for state_interface in component.command_interfaces: + for state_interface in component.state_interfaces: if state_interface.is_available: available_str = f"{bcolors.OKBLUE}[available]{bcolors.ENDC}" else: available_str = f"{bcolors.WARNING}[unavailable]{bcolors.ENDC}" - if state_interface.is_claimed: - claimed_str = f"{bcolors.OKBLUE}[claimed]{bcolors.ENDC}" - else: - claimed_str = "[unclaimed]" - - print(f"\t\t{state_interface.name} {available_str} {claimed_str}") + print(f"\t\t{state_interface.name} {available_str}") return 0 From 0e81c0ecf5ffa1e7397334accc5686f2c408561a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 20:23:33 +0100 Subject: [PATCH 09/50] Update reviewer-lottery.yml (#1001) --- .github/reviewer-lottery.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 50c707e12b..84b156f5a1 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -16,13 +16,16 @@ groups: - rosterloh - progtologist - arne48 + - christophfroehlich - DasRoteSkelett - - Serafadam + - sgmurray - harderthan - jaron-l - malapatiravi - - homalozoa - erickisos + - sachinkum0009 + - qiayuanliao + - homalozoa - anfemosa - jackcenter - VX792 @@ -31,6 +34,7 @@ groups: - aprotyas - peterdavidfagan - duringhof + - VanshGehlot - bijoua29 - - lm2292 + - LukasMacha97 - mcbed From d0c7f33c45543f6617f1e60adc58229f32cef576 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 29 Apr 2023 22:00:27 +0200 Subject: [PATCH 10/50] Fix PR template (#1000) --- .../pull_request_template.md | 10 ---------- 1 file changed, 10 deletions(-) rename .github/{PULL_REQUEST_TEMPLATE => }/pull_request_template.md (92%) diff --git a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md b/.github/pull_request_template.md similarity index 92% rename from .github/PULL_REQUEST_TEMPLATE/pull_request_template.md rename to .github/pull_request_template.md index 87f389a124..8e9178c167 100644 --- a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,13 +1,3 @@ - ---- -name: Pull request -about: Create a pull request -title: '' -labels: '' -assignees: '' - ---- - 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. From 4af55550aeb0a1634b013d734ae8f06888dbb3a6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 22:52:38 +0100 Subject: [PATCH 11/50] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 3 +++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 32 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index a81a293ff3..72ace429f6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- * Add missing build_export_depends to controller_interface (`#989 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 8372b0db3f..066ee3aac0 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 5411ab39b5..1430d21742 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index b819b8921b..4cd14a3264 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 9180ad3295..bfe66537a9 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2e1c00c372..85434fc875 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 892186bd89..b13dca226d 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 73687b1dd2..1c5f90bc85 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix verbose output of list_hardware_components (`#1004 `_) +* Contributors: Christoph Fröhlich + 3.12.1 (2023-04-14) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index ab6544a27f..863b487918 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 6ec67d7349..b4bcdfd112 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.1 (2023-04-14) ------------------- From 0be5b91e5e5223c5d86d932330206df2475660c2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 22:53:27 +0100 Subject: [PATCH 12/50] 3.12.2 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 72ace429f6..20bf5c1a92 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 510c53d057..984ac3cea8 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.12.1 + 3.12.2 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 066ee3aac0..f475bd34d0 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 6f521cedc8..65291cd864 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.12.1 + 3.12.2 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 1430d21742..4f9d2bc210 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index ae70020730..36e70d8643 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.12.1 + 3.12.2 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 4cd14a3264..28c6e0d3ca 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index b257841b68..dda55a198b 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.12.1 + 3.12.2 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index bfe66537a9..7f94f4bb05 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 081478e904..2199d4cc95 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.12.1 + 3.12.2 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 85434fc875..bf08bf8b9f 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 601ed4bdc8..4b5a76a993 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.12.1 + 3.12.2 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 b13dca226d..913ad40f64 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 568ce07a65..57a237a45b 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.12.1 + 3.12.2 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 1c5f90bc85..51fd443ac7 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- * Fix verbose output of list_hardware_components (`#1004 `_) * Contributors: Christoph Fröhlich diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 24773e9992..d0eea0133a 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.12.1 + 3.12.2 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 9455ffc0d7..212d1fa9a8 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.12.1", + version="3.12.2", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 863b487918..b142ded4db 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 07571ab6e2..7f093cb7d8 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.12.1 + 3.12.2 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 7e9127b209..8b5179fcbe 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.12.1", + version="3.12.2", 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 b4bcdfd112..8a792b4d5e 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.2 (2023-04-29) +------------------- 3.12.1 (2023-04-14) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 8297097348..602bc0af9a 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.12.1 + 3.12.2 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From adb8897a88eeef5b86460c4fb519dcf84cfea2f9 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 May 2023 08:39:43 +0200 Subject: [PATCH 13/50] [CI] Use clangformat hook and don't install it manually (#1007) * [CI] Use clangformat hhok and don't install it manually * Don't install manually clang-format --- .github/workflows/ci-format.yml | 2 +- .pre-commit-config.yaml | 9 ++------- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 2020dd2f77..83fe361f4d 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -17,7 +17,7 @@ jobs: with: python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck + run: sudo apt install -qq cppcheck - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 4af4875830..d4f6fea70b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -61,15 +61,10 @@ repos: args: ["--extend-ignore=E501"] # CPP hooks - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 hooks: - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format-14 - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] - repo: local hooks: From cb92c50a2a90b5471be0e849d2b3569b1a11524b Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 5 May 2023 20:11:05 +0200 Subject: [PATCH 14/50] docs: Fix link to hardware_components (#1009) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix link to hardware_components Fix the link to the hardware_components documentation to a working version. * use sphinx ref Co-authored-by: Christoph Fröhlich --------- Co-authored-by: Christoph Fröhlich --- hardware_interface/doc/hardware_components_userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 33230ffed1..e6a9bf5029 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -4,7 +4,7 @@ Hardware Components ------------------- Hardware components represent abstraction of physical hardware in ros2_control framework. There are three types of hardware Actuator, Sensor and System. -For details on each type check `Hardware Components description `_. +For details on each type check :ref:`overview_hardware_components` description. Guidelines and Best Practices ***************************** From e3f38ccbf8f3d7ebc56215780a430fe1a1b23cd7 Mon Sep 17 00:00:00 2001 From: bijoua29 <73511637+bijoua29@users.noreply.github.com> Date: Tue, 9 May 2023 09:39:39 -0700 Subject: [PATCH 15/50] Remove log-level argument from spawner script (#1013) Use standard ros-args instead --- .../controller_manager/spawner.py | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 24aad0ae78..8463c11b19 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -184,13 +184,6 @@ def main(args=None): default=10, type=int, ) - parser.add_argument( - "--log-level", - help="Log level for spawner node", - required=False, - choices=["debug", "info", "warn", "error", "fatal"], - default="info", - ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -200,15 +193,6 @@ def main(args=None): param_file = args.param_file controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout - log_level = args.log_level - - loglevel_to_severity = { - "debug": rclpy.logging.LoggingSeverity.DEBUG, - "info": rclpy.logging.LoggingSeverity.INFO, - "warn": rclpy.logging.LoggingSeverity.WARN, - "error": rclpy.logging.LoggingSeverity.ERROR, - "fatal": rclpy.logging.LoggingSeverity.FATAL, - } if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -218,7 +202,6 @@ def main(args=None): prefixed_controller_name = controller_namespace + "/" + controller_name node = Node("spawner_" + controller_name) - rclpy.logging.set_logger_level("spawner_" + controller_name, loglevel_to_severity[log_level]) if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() From c9709f3ce2ebf31ab5ea2c25829c422f7465f0d7 Mon Sep 17 00:00:00 2001 From: Alejandro Bordallo Date: Wed, 10 May 2023 23:19:00 +0100 Subject: [PATCH 16/50] Implement parse_bool and refactor a few (#1014) --- .../include/hardware_interface/component_parser.hpp | 3 +++ hardware_interface/src/component_parser.cpp | 7 ++++++- .../src/mock_components/generic_system.cpp | 9 ++++----- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 1d0f07d94b..9f81a2a863 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,5 +33,8 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +HARDWARE_INTERFACE_PUBLIC +bool parse_bool(const std::string & bool_string); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 14f2dee21e..46c6b5f0e0 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -224,7 +224,7 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kIsAsyncAttribute); - return attr ? strcasecmp(attr->Value(), "true") == 0 : false; + return attr ? parse_bool(attr->Value()) : false; } /// Search XML snippet from URDF for parameters. @@ -612,4 +612,9 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} + } // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d1405dac8c..04f0884607 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -25,6 +25,7 @@ #include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" @@ -74,8 +75,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i auto it = info_.hardware_parameters.find("mock_sensor_commands"); if (it != info_.hardware_parameters.end()) { - // TODO(anyone): change this to parse_bool() (see ros2_control#339) - use_mock_sensor_command_interfaces_ = it->second == "true" || it->second == "True"; + use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { @@ -83,7 +83,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("fake_sensor_commands"); if (it != info_.hardware_parameters.end()) { - use_mock_sensor_command_interfaces_ = it->second == "true" || it->second == "True"; + use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); RCUTILS_LOG_WARN_NAMED( "fake_generic_system", "Parameter 'fake_sensor_commands' has been deprecated from usage. Use" @@ -99,8 +99,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("fake_gpio_commands"); if (it != info_.hardware_parameters.end()) { - // TODO(anyone): change this to parse_bool() (see ros2_control#339) - use_fake_gpio_command_interfaces_ = it->second == "true" || it->second == "True"; + use_fake_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { From f0e72f8b6342b985198513e246c10e4c12c48443 Mon Sep 17 00:00:00 2001 From: mosfet80 Date: Sun, 14 May 2023 11:42:44 +0200 Subject: [PATCH 17/50] Update precommit libraries(#1020) --- .pre-commit-config.yaml | 6 +++--- hardware_interface/src/mock_components/generic_system.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d4f6fea70b..0ad4d0aa6c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -36,7 +36,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.4.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -49,7 +49,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 23.1.0 + rev: 23.3.0 hooks: - id: black args: ["--line-length=99"] @@ -128,7 +128,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.2.4 hooks: - id: codespell args: ['--write-changes'] diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 04f0884607..d2c2e56510 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -120,7 +120,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // its extremlly unprobably that std::distance results int this value - therefore default + // it's extremely improbable that std::distance results int this value - therefore default index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces From 5065d0c22faa021aadceea3c42407a9b9e724eb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 14 May 2023 20:53:20 +0200 Subject: [PATCH 18/50] Fix github links on control.ros.org (#1019) --- doc/index.rst | 2 ++ hardware_interface/doc/hardware_components_userdoc.rst | 2 ++ hardware_interface/doc/mock_components_userdoc.rst | 2 ++ hardware_interface/doc/writing_new_hardware_interface.rst | 2 ++ ros2controlcli/doc/userdoc.rst | 2 ++ 5 files changed, 10 insertions(+) diff --git a/doc/index.rst b/doc/index.rst index 5821b795f3..b5e9c78fbe 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/doc/index.rst + .. _ros2_control_framework: ################# diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index e6a9bf5029..c6edc7b04d 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/hardware_interface/doc/hardware_components_userdoc.rst + .. _hardware_components_userdoc: Hardware Components diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 0e74b9fb2d..3fbc1c4a14 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/hardware_interface/doc/mock_components_userdoc.rst + .. _mock_components_userdoc: Mock Components diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_interface.rst index fb748359eb..a5ece92560 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_interface.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/hardware_interface/doc/writing_new_hardware_interface.rst + .. _writing_new_hardware_interface: Writing a new hardware interface diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 6ae26df325..c608c3a3de 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/ros2controlcli/doc/userdoc.rst + .. _ros2controlcli_userdoc: Command Line Interface From 0eb319ea43b78b886ac0907ed005daf413a1dbdb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 16 May 2023 13:26:19 +0200 Subject: [PATCH 19/50] Fix GitHub link on control.ros.org (#1022) --- controller_manager/doc/userdoc.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index b23ed876b4..97d6b6e01d 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/controller_manager/doc/userdoc.rst + .. _controller_manager_userdoc: Controller Manager From 51e6aa920aaf35d1d28209b63722cf287cfd168a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A1rk=20Szitanics?= Date: Wed, 17 May 2023 22:16:26 +0200 Subject: [PATCH 20/50] Add class for thread management of async hw interfaces (#981) --- controller_manager/src/controller_manager.cpp | 3 +- .../hardware_interface/async_components.hpp | 112 +++++++++ .../hardware_interface/resource_manager.hpp | 19 +- .../include/hardware_interface/sensor.hpp | 3 + hardware_interface/src/resource_manager.cpp | 213 ++++++++++++++---- .../test/test_resource_manager.cpp | 1 + 6 files changed, 303 insertions(+), 48 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/async_components.hpp diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b912b622e2..6b6efb0a5c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -135,7 +135,8 @@ ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, const std::string & namespace_, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, namespace_, options), - resource_manager_(std::make_unique()), + resource_manager_(std::make_unique( + update_rate_, this->get_node_clock_interface())), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( 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..2fa6fd7f85 --- /dev/null +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -0,0 +1,112 @@ +// 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/duration.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" + +namespace hardware_interface +{ + +class AsyncComponentThread +{ +public: + explicit AsyncComponentThread( + Actuator * component, unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) + { + } + + explicit AsyncComponentThread( + System * component, unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) + { + } + + explicit AsyncComponentThread( + Sensor * component, unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) + { + } + + AsyncComponentThread(const AsyncComponentThread & t) = delete; + AsyncComponentThread(AsyncComponentThread && t) = default; + + ~AsyncComponentThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (write_and_read_.joinable()) + { + write_and_read_.join(); + } + } + + void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); } + + 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_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; + + // write + // read + } + 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_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; +}; + +}; // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1693e85574..a5113380e1 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -16,25 +16,28 @@ #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ #include -#include #include #include #include +#include "hardware_interface/actuator.hpp" #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" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.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" +#include "rclcpp/node.hpp" #include "rclcpp/time.hpp" namespace hardware_interface { -class ActuatorInterface; -class SensorInterface; -class SystemInterface; class ResourceStorage; +class ControllerManager; struct HardwareReadWriteStatus { @@ -46,7 +49,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: /// Default constructor for the Resource Manager. - ResourceManager(); + ResourceManager( + unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); /// Constructor for the Resource Manager. /** @@ -65,7 +70,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * "autostart_components" and "autoconfigure_components" instead. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false); + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, + unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index aaf9fcef8b..5d0677c587 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -77,6 +77,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } + private: std::unique_ptr impl_; }; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 33236afd1c..2e23c34d94 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -17,13 +17,16 @@ #include #include #include +#include #include +#include #include #include #include #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" @@ -73,10 +76,16 @@ class ResourceStorage static constexpr const char * system_interface_name = "hardware_interface::SystemInterface"; public: - ResourceStorage() + // TODO(VX792): Change this when HW ifs get their own update rate, + // because the ResourceStorage really shouldn't know about the cm's parameters + ResourceStorage( + unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), - system_loader_(pkg_name, system_interface_name) + system_loader_(pkg_name, system_interface_name), + cm_update_rate_(update_rate), + clock_interface_(clock_interface) { } @@ -192,6 +201,13 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } + + if (hardware_info_map_[hardware.get_name()].is_async) + { + async_component_threads_.emplace( + std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), + std::forward_as_tuple(&hardware, cm_update_rate_, clock_interface_)); + } } return result; } @@ -271,6 +287,7 @@ class ResourceStorage if (result) { + async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); @@ -288,6 +305,10 @@ class ResourceStorage if (result) { + if (async_component_threads_.find(hardware.get_name()) != async_component_threads_.end()) + { + async_component_threads_.at(hardware.get_name()).activate(); + } // TODO(destogl): make all command interfaces available (currently are all available) } @@ -489,54 +510,126 @@ class ResourceStorage // TODO(destogl): Propagate "false" up, if happens in initialize_hardware void load_and_initialize_actuator(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, actuators_); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto load_and_init_actuators = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, actuator_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_actuators(async_actuators_); + } + else + { + load_and_init_actuators(actuators_); + } } void load_and_initialize_sensor(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, sensors_); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto load_and_init_sensors = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, sensor_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_sensors(async_sensors_); + } + else + { + load_and_init_sensors(sensors_); + } } void load_and_initialize_system(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, systems_); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto load_and_init_systems = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, system_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_systems(async_systems_); + } + else + { + load_and_init_systems(systems_); + } } void initialize_actuator( std::unique_ptr actuator, const HardwareInfo & hardware_info) { - this->actuators_.emplace_back(Actuator(std::move(actuator))); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto init_actuators = [&](auto & container) + { + container.emplace_back(Actuator(std::move(actuator))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_actuators(async_actuators_); + } + else + { + init_actuators(actuators_); + } } void initialize_sensor( std::unique_ptr sensor, const HardwareInfo & hardware_info) { - this->sensors_.emplace_back(Sensor(std::move(sensor))); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto init_sensors = [&](auto & container) + { + container.emplace_back(Sensor(std::move(sensor))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_sensors(async_sensors_); + } + else + { + init_sensors(sensors_); + } } void initialize_system( std::unique_ptr system, const HardwareInfo & hardware_info) { - this->systems_.emplace_back(System(std::move(system))); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto init_systems = [&](auto & container) + { + container.emplace_back(System(std::move(system))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_systems(async_systems_); + } + else + { + init_systems(systems_); + } } // hardware plugins @@ -548,6 +641,10 @@ class ResourceStorage std::vector sensors_; std::vector systems_; + std::vector async_actuators_; + std::vector async_sensors_; + std::vector async_systems_; + std::unordered_map hardware_info_map_; /// Mapping between hardware and controllers that are using it (accessing data from it) @@ -567,15 +664,28 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + + /// List of async components by type + std::unordered_map async_component_threads_; + + // Update rate of the controller manager, and the clock interface of its node + // Used by async components. + unsigned int cm_update_rate_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; }; -ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} +ResourceManager::ResourceManager( + unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique(update_rate, clock_interface)) +{ +} ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool validate_interfaces, bool activate_all) -: resource_storage_(std::make_unique()) + const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique(update_rate, clock_interface)) { load_urdf(urdf, validate_interfaces); @@ -886,18 +996,20 @@ void ResourceManager::import_component( // CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() { - for (auto & component : resource_storage_->actuators_) - { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } - for (auto & component : resource_storage_->sensors_) + auto loop_and_get_state = [&](auto & container) { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } - for (auto & component : resource_storage_->systems_) - { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } + for (auto & component : container) + { + resource_storage_->hardware_info_map_[component.get_name()].state = component.get_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_); return resource_storage_->hardware_info_map_; } @@ -1056,6 +1168,24 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } + if (!found) + { + found = find_set_component_state( + std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), + resource_storage_->async_actuators_); + } + if (!found) + { + found = find_set_component_state( + std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), + resource_storage_->async_systems_); + } + if (!found) + { + found = find_set_component_state( + std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), + resource_storage_->async_sensors_); + } return result; } @@ -1144,6 +1274,7 @@ bool ResourceManager::state_interface_exists(const std::string & key) const return resource_storage_->state_interface_map_.find(key) != resource_storage_->state_interface_map_.end(); } + // END: "used only in tests and locally" // BEGIN: private methods diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 59257f4edc..e1b9e240e7 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -340,6 +340,7 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; external_component_hw_info.type = "actuator"; + external_component_hw_info.is_async = false; rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); From 3de3a598c50bda2b61d6f0c18b83277b1f2fff10 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 18 May 2023 00:27:32 +0100 Subject: [PATCH 21/50] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 7 +++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 9 +++++++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 42 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 20bf5c1a92..ff5eb22896 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index f475bd34d0..216d01ccd1 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------- diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 4f9d2bc210..d99791549b 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 28c6e0d3ca..3d507844e1 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 7f94f4bb05..4437e738ef 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index bf08bf8b9f..725033ad4e 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 913ad40f64..fc4e495d25 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 51fd443ac7..6e6164dbff 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index b142ded4db..2d893b116b 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 8a792b4d5e..3bfb9a7ff5 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.2 (2023-04-29) ------------------- From 8d47dcc0b0a35802e706252a558738bee5d967ef Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 18 May 2023 00:27:52 +0100 Subject: [PATCH 22/50] 3.13.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index ff5eb22896..29f69150d2 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 984ac3cea8..f20eccd9eb 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.12.2 + 3.13.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 216d01ccd1..d663cd3b32 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 65291cd864..246bf5f9d4 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.12.2 + 3.13.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index d99791549b..66c0285a44 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 36e70d8643..38b24b6b26 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.12.2 + 3.13.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 3d507844e1..245ae211e5 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index dda55a198b..a024ad4a27 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.12.2 + 3.13.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 4437e738ef..211828a28b 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 2199d4cc95..7bcf465d24 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.12.2 + 3.13.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 725033ad4e..d6e3effbb0 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 4b5a76a993..ddc9ae5edd 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.12.2 + 3.13.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index fc4e495d25..e5b824a921 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 57a237a45b..00c489b6a2 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.12.2 + 3.13.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 6e6164dbff..0af749a91a 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- * Fix github links on control.ros.org (`#1019 `_) * Contributors: Christoph Fröhlich diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index d0eea0133a..ca01cbe271 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.12.2 + 3.13.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 212d1fa9a8..b916f1a2af 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.12.2", + version="3.13.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 2d893b116b..f57aa7fcbd 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 7f093cb7d8..acd6496e8d 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.12.2 + 3.13.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 8b5179fcbe..7d12c68e1f 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.12.2", + version="3.13.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 3bfb9a7ff5..7dded27cc1 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-05-18) +------------------- 3.12.2 (2023-04-29) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 602bc0af9a..99d864bf6a 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.12.2 + 3.13.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 7174a1d1038bf2fbf3529409e2ea44b6ce64f759 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 19 May 2023 11:01:24 +0200 Subject: [PATCH 23/50] Use consequently 'mock' instead of 'fake'. (#1026) --- .../mock_components/generic_system.hpp | 6 +- .../src/mock_components/generic_system.cpp | 51 ++++++---- .../mock_components/test_generic_system.cpp | 93 ++++++++++++++----- 3 files changed, 107 insertions(+), 43 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 78ae1488a6..9506139c4a 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -81,12 +81,12 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector sensor_interfaces_; /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; + std::vector> sensor_mock_commands_; std::vector> sensor_states_; std::vector gpio_interfaces_; /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_fake_commands_; + std::vector> gpio_mock_commands_; std::vector> gpio_commands_; std::vector> gpio_states_; @@ -108,7 +108,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector & interfaces, std::vector> & storage, std::vector & target_interfaces, bool using_state_interfaces); - bool use_fake_gpio_command_interfaces_; + bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d2c2e56510..d490784dbd 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -85,7 +85,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i { use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); RCUTILS_LOG_WARN_NAMED( - "fake_generic_system", + "mock_generic_system", "Parameter 'fake_sensor_commands' has been deprecated from usage. Use" "'mock_sensor_commands' instead."); } @@ -95,15 +95,28 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - // check if to create fake command interface for gpio - it = info_.hardware_parameters.find("fake_gpio_commands"); + // check if to create mock command interface for gpio + it = info_.hardware_parameters.find("mock_gpio_commands"); if (it != info_.hardware_parameters.end()) { - use_fake_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); + use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { - use_fake_gpio_command_interfaces_ = false; + // 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()) + { + use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Parameter 'fake_gpio_commands' has been deprecated from usage. Use" + "'mock_gpio_commands' instead."); + } + else + { + use_mock_gpio_command_interfaces_ = false; + } } // process parameters about state following @@ -188,14 +201,14 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i index_custom_interface_with_following_offset_ = std::distance(other_interfaces_.begin(), if_it); RCUTILS_LOG_INFO_NAMED( - "fake_generic_system", "Custom interface with following offset '%s' found at index: %zu.", + "mock_generic_system", "Custom interface with following offset '%s' found at index: %zu.", custom_interface_with_following_offset_.c_str(), index_custom_interface_with_following_offset_); } else { RCUTILS_LOG_WARN_NAMED( - "fake_generic_system", + "mock_generic_system", "Custom interface with following offset '%s' does not exist. Offset will not be applied", custom_interface_with_following_offset_.c_str()); } @@ -214,7 +227,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } initialize_storage_vectors( - sensor_fake_commands_, sensor_states_, sensor_interfaces_, info_.sensors); + sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); // search for gpio interfaces for (const auto & gpio : info_.gpios) @@ -226,10 +239,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_); } - // Fake gpio command interfaces - if (use_fake_gpio_command_interfaces_) + // Mock gpio command interfaces + if (use_mock_gpio_command_interfaces_) { - initialize_storage_vectors(gpio_fake_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); } // Real gpio command interfaces else @@ -309,22 +322,22 @@ std::vector GenericSystem::export_command_ } } - // Fake sensor command interfaces + // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_fake_commands_, command_interfaces, true)) + info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); } } - // Fake gpio command interfaces (consider all state interfaces for command interfaces) - if (use_fake_gpio_command_interfaces_) + // Mock gpio command interfaces (consider all state interfaces for command interfaces) + if (use_mock_gpio_command_interfaces_) { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_fake_commands_, command_interfaces, true)) + info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -403,13 +416,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur if (use_mock_sensor_command_interfaces_) { - mirror_command_to_state(sensor_states_, sensor_fake_commands_); + mirror_command_to_state(sensor_states_, sensor_mock_commands_); } // do loopback on all gpio interfaces - if (use_fake_gpio_command_interfaces_) + if (use_mock_gpio_command_interfaces_) { - mirror_command_to_state(gpio_states_, gpio_fake_commands_); + mirror_command_to_state(gpio_states_, gpio_mock_commands_); } else { diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 9fb1aa4988..e82d7d1fb2 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -39,8 +39,9 @@ const auto PERIOD = rclcpp::Duration::from_seconds(0.01); class TestGenericSystem : public ::testing::Test { public: - void test_generic_system_with_mock_sensor_commands(std::string & urdf); void test_generic_system_with_mimic_joint(std::string & urdf); + void test_generic_system_with_mock_sensor_commands(std::string & urdf); + void test_generic_system_with_mock_gpio_commands(std::string & urdf); protected: void SetUp() override @@ -185,7 +186,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_fake_command_ = + hardware_system_2dof_with_sensor_mock_command_ = R"( @@ -214,7 +215,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_fake_command_True_ = + hardware_system_2dof_with_sensor_mock_command_True_ = R"( @@ -381,7 +382,41 @@ class TestGenericSystem : public ::testing::Test )"; - valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ = + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = + R"( + + + mock_components/GenericSystem + true + + + + + + + 3.45 + + + + + + + 2.78 + + + + + + + + + + + + +)"; + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( @@ -457,14 +492,15 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; std::string hardware_system_2dof_with_sensor_; - std::string hardware_system_2dof_with_sensor_fake_command_; - std::string hardware_system_2dof_with_sensor_fake_command_True_; + std::string hardware_system_2dof_with_sensor_mock_command_; + std::string hardware_system_2dof_with_sensor_mock_command_True_; std::string hardware_system_2dof_with_mimic_joint_; std::string hardware_system_2dof_standard_interfaces_with_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_; std::string valid_urdf_ros2_control_system_robot_with_gpio_; - std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; }; @@ -485,12 +521,12 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command_True); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True); FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint); FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); + 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); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -1070,18 +1106,18 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands(std::strin ASSERT_EQ(4.44, sty_c.get_value()); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ + + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command_True) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) { auto urdf = ros2_control_test_assets::urdf_head + - hardware_system_2dof_with_sensor_fake_command_True_ + + hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); @@ -1396,11 +1432,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) generic_system_functional_test(urdf); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command) +void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string & urdf) { - auto urdf = ros2_control_test_assets::urdf_head + - valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ + - ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // check is hardware is started @@ -1507,7 +1540,25 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_co ASSERT_EQ(2.22, gpio2_vac_c.get_value()); } -TEST_F(TestGenericSystem, sensor_with_initial_value_) +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf); +} + +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf); +} + +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; @@ -1535,7 +1586,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value_) ASSERT_EQ(0.0, force_z_s.get_value()); } -TEST_F(TestGenericSystem, gpio_with_initial_value_) +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; From 775dd6a4908a41e551c29771270d04753b0cb6d0 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 19 May 2023 12:36:15 +0200 Subject: [PATCH 24/50] Add text to assertions references (#1023) This should make debugging failed tests easier. --- .../test/test_hardware_management_srvs.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 9df237a9af..273e09d6a2 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -91,11 +91,17 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs const std::string & name, const std::string & type, const std::string & plugin_name, const uint8_t state_id, const std::string & state_label) { - EXPECT_EQ(component.name, name); - EXPECT_EQ(component.type, type); - EXPECT_EQ(component.plugin_name, plugin_name); - EXPECT_EQ(component.state.id, state_id); - EXPECT_EQ(component.state.label, state_label); + EXPECT_EQ(component.name, name) << "Component has unexpected name."; + EXPECT_EQ(component.type, 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 + << " has wrong state_label."; } void list_hardware_components_and_check( @@ -124,8 +130,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { auto it = std::find(interface_names.begin(), interface_names.end(), interfaces[i].name); EXPECT_NE(it, interface_names.end()); - EXPECT_EQ(interfaces[i].is_available, is_available_status[i]); - EXPECT_EQ(interfaces[i].is_claimed, is_claimed_status[i]); + EXPECT_EQ(interfaces[i].is_available, is_available_status[i]) + << "At " << interfaces[i].name; + EXPECT_EQ(interfaces[i].is_claimed, is_claimed_status[i]) << "At " << interfaces[i].name; } }; From 7c074302269254e3ade2639c698473351877a595 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 19 May 2023 22:08:25 +0200 Subject: [PATCH 25/50] [URDF Parser] Allow empty urdf tag, e.g., parameter (#1017) * Do not explode with empty tag * Update tests to allow empty URDF parameter to hardware interface * Test checks that empty parameter tags work and actually checks the parsed urdf. --------- Co-authored-by: Bence Magyar Co-authored-by: Dr. Denis --- hardware_interface/src/component_parser.cpp | 4 ++- .../test/test_component_parser.cpp | 36 +++++++++++++------ .../components_urdfs.hpp | 32 ++++++++--------- 3 files changed, 44 insertions(+), 28 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 46c6b5f0e0..305c7ff7aa 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -68,7 +69,8 @@ std::string get_text_for_element( const auto get_text_output = element_it->GetText(); if (!get_text_output) { - throw std::runtime_error("text not specified in the " + tag_name + " tag"); + std::cerr << "text not specified in the " << tag_name << " tag" << std::endl; + return ""; } return get_text_output; } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 97d4a77d21..b0c7c5a16d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -100,16 +100,6 @@ TEST_F(TestComponentParser, component_interface_type_empty_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); } -TEST_F(TestComponentParser, parameter_empty_throws_error) -{ - const std::string broken_urdf_string = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::invalid_urdf_ros2_control_parameter_empty + - ros2_control_test_assets::urdf_tail; - - ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); -} - TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) { std::string urdf_to_test = @@ -625,6 +615,32 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); } +TEST_F(TestComponentParser, successfully_parse_parameter_empty) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_parameter_empty + + 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, "2DOF_System_Robot_Position_Only"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); + + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position"); + + 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"); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + 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 f3e2bda2c0..a42d39a241 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 @@ -398,6 +398,21 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +const auto valid_urdf_ros2_control_parameter_empty = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + + 2 + + + + + + +)"; + // Errors const auto invalid_urdf_ros2_control_invalid_child = R"( @@ -485,23 +500,6 @@ const auto invalid_urdf_ros2_control_component_interface_type_empty = )"; -const auto invalid_urdf_ros2_control_parameter_empty = - R"( - - - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only - - 2 - - - - -1 - 1 - - - -)"; - const auto invalid_urdf2_ros2_control_illegal_size = R"( From 64d47092644103e2691eadf4ec5193b85c6ba0bc Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 22 May 2023 14:33:04 +0100 Subject: [PATCH 26/50] Bump codecov/codecov-action from 3.1.3 to 3.1.4 (#1032) --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 5ca0d46b1e..9c3a1378ee 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -44,7 +44,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.3 + - uses: codecov/codecov-action@v3.1.4 with: file: ros_ws/lcov/total_coverage.info flags: unittests From e8cfc0e827cc3780d5253cc7b77f92afd58e8f0a Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 26 May 2023 23:18:54 +0200 Subject: [PATCH 27/50] Change build badge branch for rolling to master (#1035) * Change build badge branch for rolling to master THe README was stating that for rolling the `rolling` branch should be used. However, this does not exist and instead the `master` branch should be used. * Also update documentation branches --- .github/workflows/README.md | 4 ++-- README.md | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/README.md b/.github/workflows/README.md index fd751faeef..e784883593 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -1,7 +1,7 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) **Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) diff --git a/README.md b/README.md index f19f01cc10..276ffb9e6a 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ For more, please check the [documentation](https://ros-controls.github.io/contro ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/master) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) **Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) From 590de4e346fe5ce007662f9db000e46fca6b429e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 27 May 2023 19:16:30 +0200 Subject: [PATCH 28/50] Docs: Use branch name substitution for all links (#1031) --- controller_manager/doc/userdoc.rst | 2 +- doc/index.rst | 2 +- .../doc/hardware_components_userdoc.rst | 2 +- .../doc/mock_components_userdoc.rst | 2 +- .../doc/writing_new_hardware_interface.rst | 16 +++++++++------- ros2controlcli/doc/userdoc.rst | 2 +- 6 files changed, 14 insertions(+), 12 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 97d6b6e01d..de42850b6b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/controller_manager/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/doc/userdoc.rst .. _controller_manager_userdoc: diff --git a/doc/index.rst b/doc/index.rst index b5e9c78fbe..196ea93285 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/doc/index.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/index.rst .. _ros2_control_framework: diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index c6edc7b04d..cd93f472a5 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/hardware_interface/doc/hardware_components_userdoc.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/hardware_components_userdoc.rst .. _hardware_components_userdoc: diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 3fbc1c4a14..7fd50fc9c8 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/hardware_interface/doc/mock_components_userdoc.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/mock_components_userdoc.rst .. _mock_components_userdoc: diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_interface.rst index a5ece92560..1ff4dc4420 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_interface.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/hardware_interface/doc/writing_new_hardware_interface.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_interface.rst .. _writing_new_hardware_interface: @@ -30,7 +30,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 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 :) ). 2. Include ``"hardware_interface/$interface_type$_interface.hpp"`` and ``visibility_control.h`` if you are using one. - ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check `Hardware Components description `_. + ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check :ref:`Hardware Components description `. 3. Define a unique namespace for your hardware_interface. This is usually the package name written in ``snake_case``. @@ -39,7 +39,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``. - 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*. + 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)** @@ -79,7 +79,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 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 `hardware_interface mock_components `_ section. + 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., ``/``. @@ -90,7 +90,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. - 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. + 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. 3. 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., ``/``. @@ -115,7 +115,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. 8. 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. + For details, see how it is done for mock hardware in the `ros2_control `_ package. 9. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. @@ -139,4 +139,6 @@ That's it! Enjoy writing great controllers! Useful External References --------------------------- -- `Templates and scripts for generating controllers shell `_ **NOTE**: The script is currently only recommended to use for Foxy, not compatible with the API from Galactic and onwards. +- `Templates and scripts for generating controllers shell `_ + + .. NOTE:: The script is currently only recommended to use for Foxy, not compatible with the API from Galactic and onwards. diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index c608c3a3de..dba173cfbf 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/|github_branch|/ros2controlcli/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/ros2controlcli/doc/userdoc.rst .. _ros2controlcli_userdoc: From e12cea3919b735cca3cb5b86d7af8827bd92a571 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 29 May 2023 14:38:43 +0100 Subject: [PATCH 29/50] Bump ros-tooling/setup-ros from 0.6.1 to 0.6.2 (#1039) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 9c3a1378ee..1df5175f10 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 67d1b2c5e9..57fe01c1a4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -36,7 +36,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 5a715fd503..777d193c69 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From bdf7e4437328e1998f3a75e22f5cee978825fe75 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 29 May 2023 14:39:56 +0100 Subject: [PATCH 30/50] Bump actions/setup-python from 4.6.0 to 4.6.1 (#1040) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 83fe361f4d..4cf7330f56 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.6.0 + - uses: actions/setup-python@v4.6.1 with: python-version: '3.10' - name: Install system hooks From 0637782ea436decbe46a1a42d582bd60fe8d51ef Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 May 2023 09:05:26 +0200 Subject: [PATCH 31/50] enable ReflowComments to also use ColumnLimit on comments (#1037) * enable ReflowComments to also use ColumnLimit on comments * apply the formatting changes of clang format : ReflowComments --------- Co-authored-by: Bence Magyar --- .clang-format | 2 +- .../chainable_controller_interface.hpp | 7 +- .../controller_interface_base.hpp | 19 ++- .../include/controller_interface/helpers.hpp | 26 +-- .../controller_manager/controller_manager.hpp | 13 +- .../hardware_interface/actuator_interface.hpp | 30 ++-- .../hardware_interface/component_parser.hpp | 8 +- .../hardware_interface/resource_manager.hpp | 5 +- .../hardware_interface/sensor_interface.hpp | 8 +- .../hardware_interface/system_interface.hpp | 30 ++-- hardware_interface/src/component_parser.cpp | 18 +-- .../test/test_components/test_actuator.cpp | 2 +- .../joint_limits/joint_limits_rosparam.hpp | 51 +++--- .../joint_limits_interface.hpp | 41 ++--- .../joint_limits_urdf.hpp | 4 +- .../differential_transmission.hpp | 50 +++--- .../four_bar_linkage_transmission.hpp | 151 +++++++++--------- .../simple_transmission.hpp | 37 +++-- .../transmission_interface/transmission.hpp | 42 ++--- 19 files changed, 289 insertions(+), 255 deletions(-) diff --git a/.clang-format b/.clang-format index 9c21e1e17c..698bda7da8 100644 --- a/.clang-format +++ b/.clang-format @@ -10,6 +10,6 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve ... diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 37b784011c..2bdccefdc5 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -41,7 +41,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual ~ChainableControllerInterface() = default; /** - * Control step update. Command interfaces are updated based on on reference inputs and current states. + * Control step update. Command interfaces are updated based on on reference inputs and current + * states. * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration @@ -83,7 +84,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \param[in] flag marking a switch to or from chained mode. * - * \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode. + * \returns true if controller successfully switched between "chained" and "external" mode. + * \default returns true so the method don't have to be overridden if controller can always switch + * chained mode. */ virtual bool on_set_chained_mode(bool chained_mode); diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fdaffac06f..05ee0c8830 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -74,15 +74,13 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /// Get configuration for controller's required command interfaces. /** - * Method used by the controller_manager to get the set of command interfaces used by the controller. - * Each controller can use individual method to determine interface names that in simples case - * have the following format: `/`. - * The method is called only in `inactive` or `active` state, i.e., `on_configure` has to be - * called first. - * The configuration is used to check if controller can be activated and to claim interfaces from - * hardware. - * The claimed interfaces are populated in the - * \ref ControllerInterfaceBase::command_interfaces_ "command_interfaces_" member. + * Method used by the controller_manager to get the set of command interfaces used by the + * controller. Each controller can use individual method to determine interface names that in + * simples case have the following format: `/`. The method is called only in + * `inactive` or `active` state, i.e., `on_configure` has to be called first. The configuration is + * used to check if controller can be activated and to claim interfaces from hardware. The claimed + * interfaces are populated in the \ref ControllerInterfaceBase::command_interfaces_ + * "command_interfaces_" member. * * \returns configuration of command interfaces. */ @@ -131,7 +129,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy virtual CallbackReturn on_init() = 0; /** - * Control step update. Command interfaces are updated based on on reference inputs and current states. + * Control step update. Command interfaces are updated based on on reference inputs and current + * states. * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index 45b048fc72..b571751f55 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -23,19 +23,19 @@ namespace controller_interface { /// Reorder interfaces with references according to joint names or full interface names. /** - * Method to reorder and check if all expected interfaces are provided for the joint. - * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in - * `ordered_names`. - * - * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. - * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. - * The valued inputs are list of joint names or interface full names. - * If joint names are used for ordering, \p interface_type specifies valid interface. - * If full interface names are used for ordering, \p interface_type should be empty string (""). - * \param[in] interface_type used for ordering interfaces with respect to joint names. - * \param[out] ordered_interfaces vector with ordered interfaces. - * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. - */ + * Method to reorder and check if all expected interfaces are provided for the joint. + * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in + * `ordered_names`. + * + * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. + * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. + * The valued inputs are list of joint names or interface full names. + * If joint names are used for ordering, \p interface_type specifies valid interface. + * If full interface names are used for ordering, \p interface_type should be empty string (""). + * \param[in] interface_type used for ordering interfaces with respect to joint names. + * \param[out] ordered_interfaces vector with ordered interfaces. + * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. + */ template bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & ordered_names, diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 537f0447be..582cbc5f24 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -318,8 +318,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); /** - * Clear request lists used when switching controllers. The lists are shared between "callback" and - * "control loop" threads. + * Clear request lists used when switching controllers. The lists are shared between "callback" + * and "control loop" threads. */ void clear_requests(); @@ -428,7 +428,8 @@ class ControllerManager : public rclcpp::Node * lists match and returns a reference to it * This referenced list can be modified safely until switch_updated_controller_list() * is called, at this point the RT thread may start using it at any time - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ std::vector & get_unused_list( const std::lock_guard & guard); @@ -436,7 +437,8 @@ class ControllerManager : public rclcpp::Node /// get_updated_list Returns a const reference to the most updated list. /** * \warning May or may not being used by the realtime thread, read-only reference for safety - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ const std::vector & get_updated_list( const std::lock_guard & guard) const; @@ -444,7 +446,8 @@ class ControllerManager : public rclcpp::Node /** * switch_updated_list Switches the "updated" and "outdated" lists, and waits * until the RT thread is using the new "updated" list. - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ void switch_updated_list(const std::lock_guard & guard); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 64c81b0a34..abfd8eb45a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -35,8 +35,9 @@ namespace hardware_interface /** * The typical examples are conveyors or motors. * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -46,7 +47,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. @@ -126,12 +128,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be prepared, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be prepared, or + * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -145,11 +147,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be switched to, + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be switched to, * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type perform_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 9f81a2a863..5112f7927e 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -26,10 +26,10 @@ namespace hardware_interface { /// Search XML snippet from URDF for information about a control component. /** - * \param[in] urdf string with robot's URDF - * \return vector filled with information about robot's control resources - * \throws std::runtime_error if a robot attribute or tag is not found - */ + * \param[in] urdf string with robot's URDF + * \return vector filled with information about robot's control resources + * \throws std::runtime_error if a robot attribute or tag is not found + */ HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5113380e1..4d2f995756 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -188,8 +188,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * Return list of cached controller names that use the hardware with name \p hardware_name. * - * \param[in] hardware_name the name of the hardware for which cached controllers should be returned. - * \returns list of cached controller names that depend on hardware with name \p hardware_name. + * \param[in] hardware_name the name of the hardware for which cached controllers should be + * returned. \returns list of cached controller names that depend on hardware with name \p + * hardware_name. */ std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2b2a4ce7cb..14a59e4588 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -35,8 +35,9 @@ namespace hardware_interface /** * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -46,7 +47,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 75e1a1bc29..e5c6f2f542 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -36,8 +36,9 @@ namespace hardware_interface * The common examples for these types of hardware are multi-joint systems with or without sensors * such as industrial or humanoid robots. * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -47,7 +48,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. @@ -127,12 +129,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be prepared, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be prepared, or + * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -146,11 +148,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be switched to, + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be switched to, * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type perform_command_mode_switch( diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 305c7ff7aa..1338489a07 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -304,11 +304,11 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( /// Search XML snippet from URDF for information about a control component. /** - * \param[in] component_it pointer to the iterator where component - * info should be found - * \return ComponentInfo filled with information about component - * \throws std::runtime_error if a component attribute or tag is not found - */ + * \param[in] component_it pointer to the iterator where component + * info should be found + * \return ComponentInfo filled with information about component + * \throws std::runtime_error if a component attribute or tag is not found + */ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) { ComponentInfo component; @@ -417,10 +417,10 @@ ActuatorInfo parse_transmission_actuator_from_xml(const tinyxml2::XMLElement * e /// Search XML snippet from URDF for information about a transmission. /** - * \param[in] transmission_it pointer to the iterator where transmission info should be found - * \return TransmissionInfo filled with information about transmission - * \throws std::runtime_error if an attribute or tag is not found - */ + * \param[in] transmission_it pointer to the iterator where transmission info should be found + * \return TransmissionInfo filled with information about transmission + * \throws std::runtime_error if an attribute or tag is not found + */ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transmission_it) { TransmissionInfo transmission; diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 8fec418dcf..5f9c09e95e 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -40,7 +40,7 @@ class TestActuator : public ActuatorInterface * 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;} - */ + */ return CallbackReturn::SUCCESS; } diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 5d1fd6a1f7..2f32d49271 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -28,11 +28,11 @@ namespace // utilities { /// Declare and initialize a parameter with a type. /** - * - * Wrapper function for templated node's declare_parameter() which checks if - * parameter is already declared. - * For use in all components that inherit from ControllerInterface - */ + * + * Wrapper function for templated node's declare_parameter() which checks if + * parameter is already declared. + * For use in all components that inherit from ControllerInterface + */ template auto auto_declare( const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -149,10 +149,9 @@ inline bool declare_parameters(const std::string & joint_name, const rclcpp::Nod } /** - * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node - * object. - * This is a convenience function. - * For parameters structure see the underlying `declare_parameters` function. + * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the + * lifecycle_node object. This is a convenience function. For parameters structure see the + * underlying `declare_parameters` function. * * \param[in] joint_name name of the joint for which parameters will be declared. * \param[in] lifecycle_node lifecycle node for parameters should be declared. @@ -217,10 +216,12 @@ inline bool declare_parameters( * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". * \param[in] param_itf node parameters interface of the node where parameters are specified. * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * - * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter + * exists in \p node), false otherwise. */ inline bool get_joint_limits( const std::string & joint_name, @@ -385,8 +386,9 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node Node object for which parameters should be fetched. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * * \returns True if a limits specification is found, false otherwise. */ @@ -404,8 +406,9 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * * \returns True if a limits specification is found, false otherwise. */ @@ -445,10 +448,10 @@ inline bool get_joint_limits( * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". * \param[in] param_itf node parameters interface of the node where parameters are specified. * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. - * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and - * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. \return True if a complete soft limits + * specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and \p + * soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ inline bool get_joint_limits( const std::string & joint_name, @@ -512,8 +515,8 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node Node object for which parameters should be fetched. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. * * \returns True if a soft limits specification is found, false otherwise. */ @@ -533,8 +536,8 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. * * \returns True if a soft limits specification is found, false otherwise. */ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp index 3f17ceea78..db553948d1 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -144,8 +144,8 @@ class JointSoftLimitsHandle : public JointLimitHandle joint_limits_interface::SoftJointLimits soft_limits_; }; -/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have - soft limits. */ +/** A handle used to enforce position and velocity limits of a position-controlled joint that does + not have soft limits. */ class PositionJointSaturationHandle : public JointLimitHandle { public: @@ -170,8 +170,8 @@ class PositionJointSaturationHandle : public JointLimitHandle /// Enforce position and velocity limits for a joint that is not subject to soft limits. /** - * \param[in] period Control period. - */ + * \param[in] period Control period. + */ void enforce_limits(const rclcpp::Duration & period) { if (std::isnan(prev_pos_)) @@ -209,9 +209,9 @@ class PositionJointSaturationHandle : public JointLimitHandle /// A handle used to enforce position and velocity limits of a position-controlled joint. /** - * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least - * amount of requisites on the underlying hardware platform. - * This lowers considerably the entry barrier to use it, but also implies some limitations. + * This class implements a very simple position and velocity limits enforcing policy, and tries to + * impose the least amount of requisites on the underlying hardware platform. This lowers + * considerably the entry barrier to use it, but also implies some limitations. * * Requisites * - Position (for non-continuous joints) and velocity limits specification. @@ -219,21 +219,22 @@ class PositionJointSaturationHandle : public JointLimitHandle * * Open loop nature * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for - * validity without relying on the actual position/velocity values. + * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is + * checked for validity without relying on the actual position/velocity values. * - * - Actual position values are \e not used because in some platforms there might be a substantial lag - * between sending a command and executing it (propagate command to hardware, reach control objective, - * read from hardware). + * - Actual position values are \e not used because in some platforms there might be a substantial + * lag between sending a command and executing it (propagate command to hardware, reach control + * objective, read from hardware). * - * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose - * trustworthy velocity measurements, or none at all. + * - Actual velocity values are \e not used because of the above reason, and because some platforms + * might not expose trustworthy velocity measurements, or none at all. * - * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large - * position tracking errors. Only the command is guaranteed to comply with the limits specification. + * The downside of the open loop behavior is that velocity limits will not be enforced when + * recovering from large position tracking errors. Only the command is guaranteed to comply with the + * limits specification. * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command - * velocity. + * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate + * the command velocity. */ // TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? @@ -553,8 +554,8 @@ class VelocityJointSaturationHandle : public JointLimitHandle /** * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ + * velocity-controlled joint. + */ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle { public: diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp index 6f26a997ee..80cf7f0ed4 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp @@ -27,8 +27,8 @@ namespace joint_limits_interface /** * 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. + * \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) diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 6bc89ab893..2002f6f9d7 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -30,9 +30,8 @@ namespace transmission_interface /// Implementation of a differential transmission. /** * - * This transmission relates two actuators and two joints through a differential mechanism, as illustrated - * below. - * \image html differential_transmission.png + * This transmission relates two actuators and two joints through a differential + * mechanism, as illustrated below. \image html differential_transmission.png * *
* @@ -76,28 +75,33 @@ namespace transmission_interface * *
* \f{eqnarray*}{ - * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2}) \right] \\[2.5em] - * x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} - x_{off_2}) \right] - * \f} + * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2}) + * \right] \\[2.5em] x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} - + * x_{off_2}) \right] \f} *
*
* * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates - * (cf. SimpleTransmission class documentation for a more detailed description of this variable). - * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator and joint sides - * (depicted as timing belts in the figure). - * A transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. input and output move in opposite directions. - * - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint - * directions with a given mechanical design, as they will in general not match. + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates (cf. SimpleTransmission class documentation for a more detailed description of this + * variable). + * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator + * and joint sides (depicted as timing belts in the figure). A transmission ratio can take any real + * value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. input and output move in opposite + * directions. + * - Important: Use transmission ratio signs to match this class' convention of positive + * actuator/joint directions with a given mechanical design, as they will in general not match. * - * \note This implementation currently assumes a specific layout for location of the actuators and joint axes which is - * common in robotic mechanisms. Please file an enhancement ticket if your use case does not adhere to this layout. + * \note This implementation currently assumes a specific layout for location of the actuators and + * joint axes which is common in robotic mechanisms. Please file an enhancement ticket if your use + * case does not adhere to this layout. * * \ingroup transmission_types */ @@ -126,14 +130,16 @@ class DifferentialTransmission : public Transmission /// Transform variables from actuator to joint space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void joint_to_actuator() override; 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 b8eb05e8d3..39f5df6f61 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -29,77 +29,80 @@ namespace transmission_interface { /// Implementation of a four-bar-linkage transmission. /** -* -* This transmission relates two actuators and two joints through a mechanism in which the state of the -* first joint only depends on the first actuator, while the second joint depends on both actuators, as -* illustrated below. -* Although the class name makes specific reference to the four-bar-linkage, there are other mechanical layouts -* that yield the same behavior, such as the remote actuation example also depicted below. -* \image html four_bar_linkage_transmission.png -* -*
-* -* -* -* -* -* -* -* -* -* -*
Effort
Velocity
Position
-* Actuator to joint -* -* \f{eqnarray*}{ -* \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ -* \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) -* \f} -* -* \f{eqnarray*}{ -* \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ -* \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } -* \f} -* -* \f{eqnarray*}{ -* x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ -* x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} -* \f} -*
-* Joint to actuator -* -* \f{eqnarray*}{ -* \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ -* \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } -* \f} -* -* \f{eqnarray*}{ -* \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ -* \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) -* \f} -* -* \f{eqnarray*}{ -* x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ -* x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] -* \f} -*
-*
-* -* where: -* - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. -* - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. -* - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. -* (cf. SimpleTransmission class documentation for a more detailed description of this variable). -* - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in the figure). -* A transmission ratio can take any real value \e except zero. In particular: -* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute -* value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. -* - Negative values represent a direction flip, ie. input and output move in opposite directions. -* - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint -* directions with a given mechanical design, as they will in general not match. -* -* \ingroup transmission_types -*/ + * + * This transmission relates two actuators and two joints through a mechanism in which + * the state of the first joint only depends on the first actuator, while the second joint depends + * on both actuators, as illustrated below. Although the class name makes specific reference to the + * four-bar-linkage, there are other mechanical layouts that yield the same behavior, such as the + * remote actuation example also depicted below. \image html four_bar_linkage_transmission.png + * + *
+ * + * + * + * + * + * + * + * + * + * + *
Effort
Velocity
Position
+ * Actuator to joint + * + * \f{eqnarray*}{ + * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ + * \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) + * \f} + * + * \f{eqnarray*}{ + * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ + * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} + * } \f} + * + * \f{eqnarray*}{ + * x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ + * x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} + * \f} + *
+ * Joint to actuator + * + * \f{eqnarray*}{ + * \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ + * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } + * \f} + * + * \f{eqnarray*}{ + * \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ + * \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) + * \f} + * + * \f{eqnarray*}{ + * x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ + * x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] + * \f} + *
+ *
+ * + * where: + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates. (cf. SimpleTransmission class documentation for a more detailed description of this + * variable). + * - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in + * the figure). A transmission ratio can take any real value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. input and output move in opposite + * directions. + * - Important: Use transmission ratio signs to match this class' convention of positive + * actuator/joint directions with a given mechanical design, as they will in general not match. + * + * \ingroup transmission_types + */ class FourBarLinkageTransmission : public Transmission { public: @@ -125,14 +128,16 @@ class FourBarLinkageTransmission : public Transmission /// Transform variables from actuator to joint space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index a74cc3dddd..809472ab75 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -28,9 +28,9 @@ namespace transmission_interface { /// Implementation of a simple reducer transmission. /** - * This transmission relates one actuator and one joint through a reductor (or amplifier). - * Timing belts and gears are examples of this transmission type, and are illustrated below. - * \image html simple_transmission.png + * This transmission relates one actuator and one joint through a reductor (or + * amplifier). Timing belts and gears are examples of this transmission type, and are illustrated + * below. \image html simple_transmission.png * *
* @@ -63,17 +63,20 @@ namespace transmission_interface * * * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. - * - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and input pulley - * radii for the timing belt; or the ratio between output and input teeth for the gear system. - * The transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. actuator and joint move in opposite directions. For example, - * in timing belts actuator and joint move in the same direction, while in single-stage gear systems actuator and - * joint move in opposite directions. + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates. + * - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and + * input pulley radii for the timing belt; or the ratio between output and input teeth for the gear + * system. The transmission ratio can take any real value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. actuator and joint move in opposite + * directions. For example, in timing belts actuator and joint move in the same direction, while in + * single-stage gear systems actuator and joint move in opposite directions. * * \ingroup transmission_types */ @@ -102,14 +105,16 @@ class SimpleTransmission : public Transmission /// Transform variables from actuator to joint space. /** * This method operates on the handles provided when configuring the transmission. - * To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles + * To call this method it is not required that all supported interface types are provided, e.g. + * one can supply only velocity handles */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * This method operates on the handles provided when configuring the transmission. - * To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles + * To call this method it is not required that all supported interface types are provided, e.g. + * one can supply only velocity handles */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 8ca0196c4c..697e1a4eb7 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -26,21 +26,23 @@ namespace transmission_interface { /// Abstract base class for representing mechanical transmissions. /** - * Mechanical transmissions transform effort/flow variables such that their product (power) remains constant. - * Effort variables for linear and rotational domains are \e force and \e torque; while the flow variables are - * respectively linear velocity and angular velocity. + * Mechanical transmissions transform effort/flow variables such that their product (power) remains + * constant. Effort variables for linear and rotational domains are \e force and \e torque; while + * the flow variables are respectively linear velocity and angular velocity. * - * In robotics it is customary to place transmissions between actuators and joints. This interface adheres to this - * naming to identify the input and output spaces of the transformation. - * The provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, velocity and - * position. Position is not a power variable, but the mappings can be implemented using the velocity map plus an - * integration constant representing the offset between actuator and joint zeros. + * In robotics it is customary to place transmissions between actuators and joints. This interface + * adheres to this naming to identify the input and output spaces of the transformation. The + * provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, + * velocity and position. Position is not a power variable, but the mappings can be implemented + * using the velocity map plus an integration constant representing the offset between actuator and + * joint zeros. * * \par Credit - * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow Garage Inc. + * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow + * Garage Inc. * - * \note Implementations of this interface must take care of realtime-safety if the code is to be run in realtime - * contexts, as is often the case in robot control. + * \note Implementations of this interface must take care of realtime-safety if the code is to be + * run in realtime contexts, as is often the case in robot control. */ class Transmission { @@ -55,20 +57,20 @@ class Transmission /** * \param[in] act_data Actuator-space variables. * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the + * number of transmission actuators and joints. Data vectors not used in this map can remain + * empty. */ virtual void actuator_to_joint() = 0; /// Transform \e effort variables from joint to actuator space. /** - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the + * number of transmission actuators and joints. Data vectors not used in this map can remain + * empty. + */ virtual void joint_to_actuator() = 0; /** \return Number of actuators managed by transmission, From 3b8616e1298d352408aa17458b1c5693651858a2 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Fri, 2 Jun 2023 09:44:57 -0300 Subject: [PATCH 32/50] [EOL] Foxy & Galactic (#1044) * [EOL] Foxy & Galactic - Foxy EOL date is May 2023 - Galactic EOL date was Dec 9 2022 https://docs.ros.org/en/rolling/Releases.html Signed-off-by: Alex Moriarty * fix typo --------- Signed-off-by: Alex Moriarty Co-authored-by: Bence Magyar --- .github/ISSUE_TEMPLATE/good-first-issue.md | 4 +-- .github/mergify.yml | 20 +---------- .github/workflows/README.md | 2 -- .github/workflows/foxy-abi-compatibility.yml | 20 ----------- .github/workflows/foxy-binary-build-main.yml | 26 --------------- .../workflows/foxy-semi-binary-build-main.yml | 25 -------------- .../workflows/galactic-abi-compatibility.yml | 20 ----------- .../workflows/galactic-binary-build-main.yml | 26 --------------- .../workflows/galactic-rhel-binary-build.yml | 33 ------------------- .../galactic-semi-binary-build-main.yml | 22 ------------- .github/workflows/prerelease-check.yml | 4 --- .../reusable-ros-tooling-source-build.yml | 2 +- README.md | 2 -- ros2_control-not-released.foxy.repos | 1 - ros2_control-not-released.galactic.repos | 1 - ros2_control.foxy.repos | 9 ----- ros2_control.galactic.repos | 9 ----- 17 files changed, 4 insertions(+), 222 deletions(-) delete mode 100644 .github/workflows/foxy-abi-compatibility.yml delete mode 100644 .github/workflows/foxy-binary-build-main.yml delete mode 100644 .github/workflows/foxy-semi-binary-build-main.yml delete mode 100644 .github/workflows/galactic-abi-compatibility.yml delete mode 100644 .github/workflows/galactic-binary-build-main.yml delete mode 100644 .github/workflows/galactic-rhel-binary-build.yml delete mode 100644 .github/workflows/galactic-semi-binary-build-main.yml delete mode 100644 ros2_control-not-released.foxy.repos delete mode 100644 ros2_control-not-released.galactic.repos delete mode 100644 ros2_control.foxy.repos delete mode 100644 ros2_control.galactic.repos diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index a4f155ffda..5ff89fff5a 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,7 @@ 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! -- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/galactic/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). +- [ ] 🗄️ **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 Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). - [ ] 🍴 **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) @@ -54,7 +54,7 @@ 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: * [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) -* [ROS2 Tutorials](https://docs.ros.org/en/galactic/Tutorials.html) +* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) **Good luck with your first issue!** diff --git a/.github/mergify.yml b/.github/mergify.yml index e62f958961..49d2acedfa 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -2,30 +2,12 @@ pull_request_rules: - name: Backport to humble at reviewers discretion conditions: - base=master - - "label=humble-galactic" + - "label=backport-humble" actions: backport: branches: - humble - - name: Backport to galactic at reviewers discretion - conditions: - - base=master - - "label=backport-galactic" - actions: - backport: - branches: - - galactic - - - name: Backport to foxy at reviewers discretion - conditions: - - base=master - - "label=backport-foxy" - actions: - backport: - branches: - - foxy - - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/README.md b/.github/workflows/README.md index e784883593..36caf0256f 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -3,5 +3,3 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml deleted file mode 100644 index 7ce17effd0..0000000000 --- a/.github/workflows/foxy-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Foxy - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: foxy - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build-main.yml b/.github/workflows/foxy-binary-build-main.yml deleted file mode 100644 index 0d74ce64e4..0000000000 --- a/.github/workflows/foxy-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: main - upstream_workspace: ros2_control-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-main.yml b/.github/workflows/foxy-semi-binary-build-main.yml deleted file mode 100644 index 68e709cc00..0000000000 --- a/.github/workflows/foxy-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: main - upstream_workspace: ros2_control.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml deleted file mode 100644 index 06a48ef9c7..0000000000 --- a/.github/workflows/galactic-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Galactic - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: galactic - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-binary-build-main.yml b/.github/workflows/galactic-binary-build-main.yml deleted file mode 100644 index f50c773444..0000000000 --- a/.github/workflows/galactic-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Galactic Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: galactic - ros_repo: main - upstream_workspace: ros2_control-not-released.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml deleted file mode 100644 index ca0b30382a..0000000000 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Galactic RHEL Binary Build -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - galactic_rhel_binary: - name: Galactic RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: galactic - container: ghcr.io/ros-controls/ros:galactic-rhel - steps: - - uses: actions/checkout@v3 - with: - path: src/ros2_control - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_control - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test diff --git a/.github/workflows/galactic-semi-binary-build-main.yml b/.github/workflows/galactic-semi-binary-build-main.yml deleted file mode 100644 index 0578366a45..0000000000 --- a/.github/workflows/galactic-semi-binary-build-main.yml +++ /dev/null @@ -1,22 +0,0 @@ -name: Galactic Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: galactic - ros_repo: main - upstream_workspace: ros2_control.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index b9460bda47..5e7326e510 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -9,8 +9,6 @@ on: default: 'rolling' type: choice options: - - foxy - - galactic - humble - rolling branch: @@ -19,8 +17,6 @@ on: default: 'master' type: choice options: - - foxy - - galactic - humble - master diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 777d193c69..46b998c326 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -14,7 +14,7 @@ on: required: true type: string ros2_repo_branch: - description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' + description: 'Branch in the ros2/ros2 repository from which ".repos" should be used. Possible values: master (Rolling), humble.' default: 'master' required: false type: string diff --git a/README.md b/README.md index 276ffb9e6a..e5ba217d35 100644 --- a/README.md +++ b/README.md @@ -12,8 +12,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) [Detailed build status](.github/workflows/README.md) diff --git a/ros2_control-not-released.foxy.repos b/ros2_control-not-released.foxy.repos deleted file mode 100644 index 56f46b6f79..0000000000 --- a/ros2_control-not-released.foxy.repos +++ /dev/null @@ -1 +0,0 @@ -repositories: diff --git a/ros2_control-not-released.galactic.repos b/ros2_control-not-released.galactic.repos deleted file mode 100644 index 56f46b6f79..0000000000 --- a/ros2_control-not-released.galactic.repos +++ /dev/null @@ -1 +0,0 @@ -repositories: diff --git a/ros2_control.foxy.repos b/ros2_control.foxy.repos deleted file mode 100644 index a52f653b54..0000000000 --- a/ros2_control.foxy.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - ros-controls/realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel - ros-controls/control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel diff --git a/ros2_control.galactic.repos b/ros2_control.galactic.repos deleted file mode 100644 index afb1cbf640..0000000000 --- a/ros2_control.galactic.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - ros-controls/realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel - ros-controls/control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel From e89fbe7804c317d182995db3e19457ead836c94f Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Sat, 3 Jun 2023 22:48:30 +0200 Subject: [PATCH 33/50] Added workflows, badges and repos file for ROS Iron (#1034) Since ros2_control is also released for iron, it would make sense to add workflows for iron. --- .github/workflows/README.md | 1 + .github/workflows/iron-abi-compatibility.yml | 20 ++++++++++++ .github/workflows/iron-binary-build-main.yml | 26 +++++++++++++++ .../workflows/iron-binary-build-testing.yml | 26 +++++++++++++++ .github/workflows/iron-rhel-binary-build.yml | 32 +++++++++++++++++++ .../workflows/iron-semi-binary-build-main.yml | 25 +++++++++++++++ .../iron-semi-binary-build-testing.yml | 25 +++++++++++++++ .github/workflows/iron-source-build.yml | 19 +++++++++++ .github/workflows/prerelease-check.yml | 2 ++ README.md | 1 + ros2_control-not-released.iron.repos | 1 + ros2_control.iron.repos | 9 ++++++ 12 files changed, 187 insertions(+) create mode 100644 .github/workflows/iron-abi-compatibility.yml create mode 100644 .github/workflows/iron-binary-build-main.yml create mode 100644 .github/workflows/iron-binary-build-testing.yml create mode 100644 .github/workflows/iron-rhel-binary-build.yml create mode 100644 .github/workflows/iron-semi-binary-build-main.yml create mode 100644 .github/workflows/iron-semi-binary-build-testing.yml create mode 100644 .github/workflows/iron-source-build.yml create mode 100644 ros2_control-not-released.iron.repos create mode 100644 ros2_control.iron.repos diff --git a/.github/workflows/README.md b/.github/workflows/README.md index 36caf0256f..5e8c8ef0fd 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -2,4 +2,5 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Iron** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml?branch=master)
[![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-testing.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-testing.yml?branch=master)
[![Iron Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml new file mode 100644 index 0000000000..f92166b0c1 --- /dev/null +++ b/.github/workflows/iron-abi-compatibility.yml @@ -0,0 +1,20 @@ +name: Iron - ABI Compatibility Check +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: iron + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml new file mode 100644 index 0000000000..be225cc0e1 --- /dev/null +++ b/.github/workflows/iron-binary-build-main.yml @@ -0,0 +1,26 @@ +name: Iron Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: iron + ros_repo: main + upstream_workspace: ros2_control-not-released.iron.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml new file mode 100644 index 0000000000..c44786c7e2 --- /dev/null +++ b/.github/workflows/iron-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Iron Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: iron + ros_repo: testing + upstream_workspace: ros2_control-not-released.iron.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml new file mode 100644 index 0000000000..afcdf332d4 --- /dev/null +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -0,0 +1,32 @@ +name: Iron RHEL Binary Build +on: + workflow_dispatch: + branches: + - iron +# pull_request: +# branches: +# - iron +# push: +# branches: +# - iron +# schedule: +# # Run every day to detect flakiness and broken dependencies +# - cron: '03 1 * * *' + +jobs: + iron_rhel_binary: + name: Iron RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: iron + container: ghcr.io/ros-controls/ros:iron-rhel + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_control + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_control + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml new file mode 100644 index 0000000000..1d72d06149 --- /dev/null +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -0,0 +1,25 @@ +name: Iron Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: iron + ros_repo: main + upstream_workspace: ros2_control.iron.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml new file mode 100644 index 0000000000..48707cd914 --- /dev/null +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Iron Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: iron + ros_repo: testing + upstream_workspace: ros2_control.iron.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml new file mode 100644 index 0000000000..31eccd1bef --- /dev/null +++ b/.github/workflows/iron-source-build.yml @@ -0,0 +1,19 @@ +name: Iron Source Build +on: + workflow_dispatch: + branches: + - master + push: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +jobs: + source: + uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + with: + ros_distro: iron + ref: master + ros2_repo_branch: iron diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 5e7326e510..eefb15ae87 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -10,6 +10,7 @@ on: type: choice options: - humble + - iron - rolling branch: description: 'Chose branch for distro' @@ -18,6 +19,7 @@ on: type: choice options: - humble + - iron - master jobs: diff --git a/README.md b/README.md index e5ba217d35..7981031a13 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,7 @@ For more, please check the [documentation](https://ros-controls.github.io/contro ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Iron** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) [Detailed build status](.github/workflows/README.md) diff --git a/ros2_control-not-released.iron.repos b/ros2_control-not-released.iron.repos new file mode 100644 index 0000000000..56f46b6f79 --- /dev/null +++ b/ros2_control-not-released.iron.repos @@ -0,0 +1 @@ +repositories: diff --git a/ros2_control.iron.repos b/ros2_control.iron.repos new file mode 100644 index 0000000000..c93d8f4ef6 --- /dev/null +++ b/ros2_control.iron.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master + ros-controls/control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: master From bc8052a6d48bc786eb7ffe17cfca9af92f583332 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sun, 4 Jun 2023 20:25:36 +0200 Subject: [PATCH 34/50] [MockHardware] Enable disabling of command to simulate HW failures. (#1027) --- .../doc/mock_components_userdoc.rst | 5 ++ .../mock_components/generic_system.hpp | 2 + .../src/mock_components/generic_system.cpp | 19 ++++++ .../mock_components/test_generic_system.cpp | 67 +++++++++++++++++++ 4 files changed, 93 insertions(+) diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 7fd50fc9c8..f143ea816c 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -27,6 +27,11 @@ Features: Parameters ,,,,,,,,,, +disable_commands (optional; boolean; default: false) + Disables mirroring commands to states. + This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface. + Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration. + mock_sensor_commands (optional; boolean; default: false) Creates fake command interfaces for faking sensor measurements with an external command. Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 9506139c4a..c244ee1254 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -114,6 +114,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste double position_state_following_offset_; std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; + + bool command_propagation_disabled_; }; typedef GenericSystem GenericRobot; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d490784dbd..13a5a8b679 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -119,6 +119,18 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } + // check if there is parameter that disables commands + // this way we simulate disconnected driver + it = info_.hardware_parameters.find("disable_commands"); + if (it != info.hardware_parameters.end()) + { + command_propagation_disabled_ = hardware_interface::parse_bool(it->second); + } + else + { + command_propagation_disabled_ = false; + } + // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -359,6 +371,13 @@ std::vector GenericSystem::export_command_ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + if (command_propagation_disabled_) + { + 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) { for (size_t i = start_index; i < states_.size(); ++i) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index e82d7d1fb2..8e78ade7fb 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -483,6 +483,24 @@ class TestGenericSystem : public ::testing::Test )"; + + disabled_commands_ = + R"( + + + fake_components/GenericSystem + True + + + + + + 3.45 + + + + +)"; } std::string hardware_robot_2dof_; @@ -503,6 +521,7 @@ class TestGenericSystem : public ::testing::Test std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; + std::string disabled_commands_; }; // Forward declaration @@ -1604,3 +1623,51 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ASSERT_EQ(1, state.get_value()); } + +TEST_F(TestGenericSystem, disabled_commands_flag_is_active) +{ + auto urdf = + ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; + TestableResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/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")); + + // 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::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + + // set some new values in commands + j1p_c.set_value(0.11); + + // State values should not be changed + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + + // read() also does not change values + rm.read(TIME, PERIOD); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); +} From d87ab20417dbf9400bcbf9f012fe4e4b479f8dff Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 5 Jun 2023 15:49:47 +0200 Subject: [PATCH 35/50] Bump uesteibar/reviewer-lottery from 2 to 3 (#1048) Bumps [uesteibar/reviewer-lottery](https://github.com/uesteibar/reviewer-lottery) from 2 to 3. - [Release notes](https://github.com/uesteibar/reviewer-lottery/releases) - [Commits](https://github.com/uesteibar/reviewer-lottery/compare/v2...v3) --- updated-dependencies: - dependency-name: uesteibar/reviewer-lottery dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/reviewer_lottery.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 94f3d9bde6..772809825f 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -8,6 +8,6 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: uesteibar/reviewer-lottery@v2 + - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} From 50ad772ab2f57225a37ab2df3adf1baa384ca8a8 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 5 Jun 2023 15:50:10 +0200 Subject: [PATCH 36/50] Bump ros-tooling/action-ros-ci from 0.3.0 to 0.3.2 (#1047) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.3.0 to 0.3.2. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.3.0...0.3.2) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 1df5175f10..97f5b4373b 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 46b998c326..faadc4e9d9 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From d299208e8e2c6b927adbf27391f68aced974892c Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 7 Jun 2023 23:12:26 +0200 Subject: [PATCH 37/50] =?UTF-8?q?[CM]=20Use=20`robot=5Fdescription`=20topi?= =?UTF-8?q?c=20instead=20of=20parameter=20and=20don't=20crash=20on=20empty?= =?UTF-8?q?=20URDF=20=F0=9F=A6=BF=20(#940)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * on startup wait for robot description, however allow receiving later --------- Co-authored-by: Dr. Denis --- controller_manager/CMakeLists.txt | 11 +++ .../controller_manager/controller_manager.hpp | 8 ++ controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 58 +++++++++++- .../test/controller_manager_test_common.hpp | 57 +++++++++--- .../test_controller_manager_urdf_passing.cpp | 88 +++++++++++++++++++ .../doc/mock_components_userdoc.rst | 2 +- .../hardware_interface/resource_manager.hpp | 12 +++ hardware_interface/src/resource_manager.cpp | 3 + .../test/test_resource_manager.cpp | 28 ++++++ 10 files changed, 253 insertions(+), 15 deletions(-) create mode 100644 controller_manager/test/test_controller_manager_urdf_passing.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1d9b062efa..1118856e48 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -143,6 +144,16 @@ if(BUILD_TESTING) 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 + 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 diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 582cbc5f24..631d5173aa 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -51,6 +51,8 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" namespace controller_manager { @@ -82,6 +84,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + CONTROLLER_MANAGER_PUBLIC + void robot_description_callback(const std_msgs::msg::String & msg); + CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); @@ -316,6 +321,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); + void subscribe_to_robot_description_topic(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -504,6 +510,8 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; + struct SwitchParams { bool do_switch = {false}; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 246bf5f9d4..c9dd319577 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -26,6 +26,7 @@ ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6b6efb0a5c..69a3bbb5f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -151,13 +151,20 @@ ControllerManager::ControllerManager( } std::string robot_description = ""; + // TODO(destogl): remove support at the end of 2023 get_parameter("robot_description", robot_description); if (robot_description.empty()) { - throw std::runtime_error("Unable to initialize resource manager, no robot description found."); + subscribe_to_robot_description_topic(); + } + else + { + RCLCPP_WARN( + get_logger(), + "[Deprecated] Passing the robot description parameter directly to the control_manager node " + "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); + init_resource_manager(robot_description); } - - init_resource_manager(robot_description); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( @@ -183,12 +190,55 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + + subscribe_to_robot_description_topic(); + diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); } +void ControllerManager::subscribe_to_robot_description_topic() +{ + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO( + get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); + robot_description_subscription_ = create_subscription( + "~/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); +} + +void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) +{ + RCLCPP_INFO(get_logger(), "Received robot description file."); + RCLCPP_DEBUG( + get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); + // TODO(Manuel): errors should probably be caught since we don't want controller_manager node + // to die if a non valid urdf is passed. However, should maybe be fine tuned. + try + { + if (resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; + } + init_resource_manager(robot_description.data.c_str()); + } + catch (std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + get_logger(), + "The published robot description file (urdf) seems not to be genuine. The following error " + "was caught:" + << e.what()); + } +} + void ControllerManager::init_resource_manager(const std::string & robot_description) { // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... @@ -199,7 +249,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript std::vector configure_components_on_start = std::vector({}); if (get_parameter("configure_components_on_start", configure_components_on_start)) { - RCLCPP_WARN_STREAM( + RCLCPP_WARN( get_logger(), "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " "hardware_spawner instead."); diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index b6e6acac0e..78c3fcb06b 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,15 +22,20 @@ #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/utilities.hpp" +#include "std_msgs/msg/string.hpp" + #include "ros2_control_test_assets/descriptions.hpp" #include "test_controller_failed_init/test_controller_failed_init.hpp" @@ -60,21 +65,51 @@ template class ControllerManagerFixture : public ::testing::Test { public: + explicit ControllerManagerFixture( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const bool & pass_urdf_as_parameter = false) + : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) + { + executor_ = std::make_shared(); + // We want to be able to create a ResourceManager where no urdf file has been passed to + if (robot_description_.empty()) + { + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + } + else + { + // can be removed later, needed if we want to have the deprecated way of passing the robot + // description file to the controller manager covered by tests + if (pass_urdf_as_parameter_) + { + cm_ = std::make_shared( + std::make_unique(robot_description_, true, true), + executor_, TEST_CM_NAME); + } + else + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + + // this is just a workaround to skip passing + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + // mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); + } + } + } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } static void TearDownTestCase() { rclcpp::shutdown(); } - void SetUp() - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf, true, true), - executor_, TEST_CM_NAME); - run_updater_ = false; - } + void SetUp() override { run_updater_ = false; } - void TearDown() { stopCmUpdater(); } + void TearDown() override { stopCmUpdater(); } void startCmUpdater() { @@ -120,6 +155,8 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; + const std::string robot_description_; + const bool pass_urdf_as_parameter_; }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp new file mode 100644 index 0000000000..102cbdfbd4 --- /dev/null +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -0,0 +1,88 @@ +// Copyright 2020 Open Source Robotics Foundation, 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. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" + +#include "ros2_control_test_assets/descriptions.hpp" + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + 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); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + // create cm with no urdf + TestControllerManagerWithTestableCM() + : ControllerManagerFixture("", false) + { + } +}; + +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + // mimic callback + auto msg = std_msgs::msg::String(); + 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()); +} + +INSTANTIATE_TEST_SUITE_P( + test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index f143ea816c..573037a58d 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -28,7 +28,7 @@ Parameters ,,,,,,,,,, disable_commands (optional; boolean; default: false) - Disables mirroring commands to states. + Disables mirroring commands to states. This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface. Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 4d2f995756..4ea3ae9a5f 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -90,6 +90,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void load_urdf(const std::string & urdf, bool validate_interfaces = true); + /** + * @brief if the resource manager load_urdf(...) function has been called this returns true. + * We want to permit to load the urdf later on but we currently don't want to permit multiple + * calls to load_urdf (reloading/loading different urdf). + * + * @return true if resource manager's load_urdf() has been already called. + * @return false if resource manager's load_urdf() has not been yet called. + */ + bool is_urdf_already_loaded() const; + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -413,6 +423,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + + bool is_urdf_loaded__ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e23c34d94..f32d24f890 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -703,6 +703,7 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -741,6 +742,8 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } + // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index e1b9e240e7..aedf2b862d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -212,6 +212,34 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +{ + TestableResourceManager rm; + EXPECT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + 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, resource_claiming) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); From 1b83a5b5c06fa491a96f67ac338a0c16a8112987 Mon Sep 17 00:00:00 2001 From: gwalck Date: Tue, 13 Jun 2023 16:57:37 +0200 Subject: [PATCH 38/50] Add -Wconversion flag to protect future developments (#1053) --- controller_interface/CMakeLists.txt | 2 +- controller_manager/CMakeLists.txt | 2 +- hardware_interface/CMakeLists.txt | 2 +- hardware_interface/src/component_parser.cpp | 6 ++++-- joint_limits/CMakeLists.txt | 2 +- transmission_interface/CMakeLists.txt | 2 +- transmission_interface/test/random_generator_utils.hpp | 2 +- 7 files changed, 10 insertions(+), 8 deletions(-) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 034556d19f..88a54d5c7a 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(controller_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1118856e48..77ba823e1f 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(controller_manager LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 77eec3ec86..6548d6186c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(hardware_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 1338489a07..8e9b6bf16b 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -367,7 +367,8 @@ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * comp component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); component.command_interfaces.back().data_type = parse_data_type_attribute(command_interfaces_it); - component.command_interfaces.back().size = parse_size_attribute(command_interfaces_it); + component.command_interfaces.back().size = + static_cast(parse_size_attribute(command_interfaces_it)); command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } @@ -377,7 +378,8 @@ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * comp { component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it)); component.state_interfaces.back().data_type = parse_data_type_attribute(state_interfaces_it); - component.state_interfaces.back().size = parse_size_attribute(state_interfaces_it); + component.state_interfaces.back().size = + static_cast(parse_size_attribute(state_interfaces_it)); state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 9ec9221220..82467514a3 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_limits LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 8ae2cc224c..d4674366e9 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(transmission_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/transmission_interface/test/random_generator_utils.hpp b/transmission_interface/test/random_generator_utils.hpp index e6088f1b2f..fbacd1ba3b 100644 --- a/transmission_interface/test/random_generator_utils.hpp +++ b/transmission_interface/test/random_generator_utils.hpp @@ -31,7 +31,7 @@ struct RandomDoubleGenerator public: RandomDoubleGenerator(double min_val, double max_val) : min_val_(min_val), max_val_(max_val) { - srand(time(nullptr)); + srand(static_cast(time(nullptr))); } double operator()() { From af6d8e2c9af76710bb1e4a68ebf5636b034576d3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Jun 2023 19:28:39 +0100 Subject: [PATCH 39/50] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 9 +++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 11 +++++++++++ joint_limits/CHANGELOG.rst | 6 ++++++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 6 ++++++ 10 files changed, 57 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 29f69150d2..a63c8594b2 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index d663cd3b32..9b5106c794 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 66c0285a44..92f836c269 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-05-18) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 245ae211e5..e3c52c0e1c 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 211828a28b..e713a772bf 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index d6e3effbb0..bf2fbc3b8a 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-05-18) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index e5b824a921..040695bdb4 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [URDF Parser] Allow empty urdf tag, e.g., parameter (`#1017 `_) +* Contributors: Felix Exner (fexner) + 3.13.0 (2023-05-18) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 0af749a91a..fecf6f29fb 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index f57aa7fcbd..a2f416e9eb 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-05-18) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 7dded27cc1..537ccf36ac 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------- From 99deef8b0e7eb96eafc88d6be2d017d43c1a95df Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Jun 2023 19:29:21 +0100 Subject: [PATCH 40/50] 3.14.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index a63c8594b2..d938aa8234 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/controller_interface/package.xml b/controller_interface/package.xml index f20eccd9eb..aedd5cf2f5 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.13.0 + 3.14.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 9b5106c794..3a3447e908 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c9dd319577..c08f12cb16 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.13.0 + 3.14.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 92f836c269..d0dd4d6233 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-06-14) +------------------- 3.13.0 (2023-05-18) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 38b24b6b26..186ecab6c4 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.13.0 + 3.14.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index e3c52c0e1c..8acb46a46e 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a024ad4a27..54618f05c9 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.13.0 + 3.14.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index e713a772bf..c0e9c1cc43 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 7bcf465d24..0a4990667e 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.13.0 + 3.14.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index bf2fbc3b8a..fd2b295eb9 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-06-14) +------------------- 3.13.0 (2023-05-18) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index ddc9ae5edd..a45fbc7c99 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.13.0 + 3.14.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 040695bdb4..1ae3a3c626 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-06-14) +------------------- * [URDF Parser] Allow empty urdf tag, e.g., parameter (`#1017 `_) * Contributors: Felix Exner (fexner) diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 00c489b6a2..8a3c6abf22 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.13.0 + 3.14.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index fecf6f29fb..6724a9b175 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-06-14) +------------------- * Docs: Use branch name substitution for all links (`#1031 `_) * Contributors: Christoph Fröhlich diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index ca01cbe271..f8b8caa615 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.13.0 + 3.14.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index b916f1a2af..6f6a49199a 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.13.0", + version="3.14.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index a2f416e9eb..30df32e8fd 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-06-14) +------------------- 3.13.0 (2023-05-18) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index acd6496e8d..0bb93f0838 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.13.0 + 3.14.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 7d12c68e1f..1dd0977c7a 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.13.0", + version="3.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 537ccf36ac..cdc7b97801 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 99d864bf6a..bce59f3a4a 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.13.0 + 3.14.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From dc01ae33df33fae6a0f3a142cd2613208e3b78d4 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 20 Jun 2023 21:21:35 +0200 Subject: [PATCH 41/50] [CM] Improve output when using robot description topic and give output about correct topic even remapped. (#1059) --- controller_manager/src/controller_manager.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 69a3bbb5f3..83604cd249 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -203,16 +203,17 @@ void ControllerManager::subscribe_to_robot_description_topic() { // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) - RCLCPP_INFO( - get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); 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()); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { - RCLCPP_INFO(get_logger(), "Received robot description file."); + RCLCPP_INFO(get_logger(), "Received robot description from topic."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); // TODO(Manuel): errors should probably be caught since we don't want controller_manager node From e14497e9f581197a70ba8bf50fd2efac8c603281 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 20 Jun 2023 22:07:36 +0200 Subject: [PATCH 42/50] Improve list hardware components output and code for better readability. (#1060) --- .../ros2controlcli/verb/list_hardware_components.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 54dff21bb6..8a5884f2cb 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -40,7 +40,7 @@ def main(self, *, args): for idx, component in enumerate(hardware_components.component): print( - f"Hardware Component {idx}\n\tname: {component.name}\n\ttype: {component.type}" + f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}" ) if hasattr(component, "plugin_name"): plugin_name = component.plugin_name @@ -48,7 +48,9 @@ def main(self, *, args): plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" print( - f"\tplugin name: {plugin_name}\n\tstate: id={component.state.id} label={component.state.label}\n\tcommand interfaces" + f"\tplugin name: {plugin_name}\n" + f"\tstate: id={component.state.id} label={component.state.label}\n" + f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: if cmd_interface.is_available: From db34dfaf2d79fc8afcd460d9ae16f02c43265a2a Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Wed, 21 Jun 2023 09:34:49 +0200 Subject: [PATCH 43/50] Ensure instantiation of hardware classes work for python bindings (#1058) --- hardware_interface/CMakeLists.txt | 4 +++ .../include/hardware_interface/system.hpp | 2 ++ .../test/test_inst_hardwares.cpp | 34 +++++++++++++++++++ 3 files changed, 40 insertions(+) create mode 100644 hardware_interface/test/test_inst_hardwares.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 6548d6186c..2d6c72ffae 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -80,6 +80,10 @@ if(BUILD_TESTING) target_include_directories(test_macros PRIVATE include) ament_target_dependencies(test_macros rcpputils) + ament_add_gmock(test_inst_hardwares test/test_inst_hardwares.cpp) + target_link_libraries(test_inst_hardwares hardware_interface) + ament_target_dependencies(test_inst_hardwares rcpputils) + ament_add_gmock(test_joint_handle test/test_handle.cpp) target_link_libraries(test_joint_handle hardware_interface) ament_target_dependencies(test_joint_handle rcpputils) diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 4c9ae67ae2..ece14f814d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -35,6 +35,8 @@ class SystemInterface; class System final { public: + System() = default; + HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); diff --git a/hardware_interface/test/test_inst_hardwares.cpp b/hardware_interface/test/test_inst_hardwares.cpp new file mode 100644 index 0000000000..ddd3aea0ad --- /dev/null +++ b/hardware_interface/test/test_inst_hardwares.cpp @@ -0,0 +1,34 @@ +// Copyright 2023 LAAS CNRS +// +// 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 "hardware_interface/actuator.hpp" +#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" + +class TestInstantiationHardwares : public ::testing::Test +{ +protected: + static void SetUpTestCase() {} +}; + +TEST_F(TestInstantiationHardwares, build_actuator) { hardware_interface::Actuator anActuator; } + +TEST_F(TestInstantiationHardwares, build_sensor) { hardware_interface::Sensor aSensor; } + +TEST_F(TestInstantiationHardwares, build_system) { hardware_interface::System aSystem; } From cf4448d92d6644c04f060452e309d362ebbd118b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 23 Jun 2023 20:23:46 +0200 Subject: [PATCH 44/50] Enable setting of initial state in HW compoments (#1046) --- controller_manager/doc/userdoc.rst | 28 ++-- controller_manager/src/controller_manager.cpp | 98 +++++++++++--- .../test/test_hardware_management_srvs.cpp | 126 +++++++++++++----- .../hardware_interface/resource_manager.hpp | 15 +-- hardware_interface/src/resource_manager.cpp | 21 --- 5 files changed, 191 insertions(+), 97 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index de42850b6b..a05f6a3afc 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -39,21 +39,27 @@ The limits will be applied after you log out and in again. Parameters ----------- -activate_components_on_start (optional; list; default: empty) - Define which hardware components should be activated when controller manager is started. +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``. - All other components will stay ``UNCONFIGURED``. - If this and ``configure_components_on_start`` are empty, all available components will be activated. - If this or ``configure_components_on_start`` are not empty, any component not in either list will be in unconfigured state. + 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 -configure_components_on_start (optional; list; default: empty) - Define which hardware components should be configured when controller manager is started. - The names of the components are defined as attribute of ````-tag in ``robot_description``. - All other components will stay ``UNCONFIGURED``. - If this and ``activate_components_on_start`` are empty, all available components will be activated. - If this or ``activate_components_on_start`` are not empty, any component not in either list will be in unconfigured state. + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +hardware_components_initial_state.unconfigured (optional; list; default: empty) + Defines which hardware components will be only loaded immediately when controller manager is started. +hardware_components_initial_state.inactive (optional; list; default: empty) + Defines which hardware components will be configured immediately when controller manager is started. robot_description (mandatory; string) String with the URDF string as robot description. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 83604cd249..58f91a786d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -245,30 +245,85 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... resource_manager_->load_urdf(robot_description); + // Get all components and if they are not defined in parameters activate them automatically + auto components_to_activate = resource_manager_->get_components_status(); + using lifecycle_msgs::msg::State; + auto set_components_to_state = + [&](const std::string & parameter_name, rclcpp_lifecycle::State state) + { + std::vector components_to_set = std::vector({}); + if (get_parameter(parameter_name, components_to_set)) + { + 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()); + resource_manager_->set_component_state(component, state); + components_to_activate.erase(component); + } + } + } + }; + + // unconfigured (loaded only) + set_components_to_state( + "hardware_components_initial_state.unconfigured", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); + + // inactive (configured) + // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) std::vector configure_components_on_start = std::vector({}); - if (get_parameter("configure_components_on_start", configure_components_on_start)) + get_parameter("configure_components_on_start", configure_components_on_start); + if (!configure_components_on_start.empty()) { RCLCPP_WARN( get_logger(), - "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " - "hardware_spawner instead."); - rclcpp_lifecycle::State inactive_state( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - for (const auto & component : configure_components_on_start) - { - resource_manager_->set_component_state(component, inactive_state); - } + "Parameter 'configure_components_on_start' is deprecated. " + "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial " + "state to 'inactive'. Don't use this parameters in combination with the new " + "'hardware_interface_state_after_start' parameter structure."); + set_components_to_state( + "configure_components_on_start", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); + } + // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023) + else + { + set_components_to_state( + "hardware_components_initial_state.inactive", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); } + // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) std::vector activate_components_on_start = std::vector({}); - if (get_parameter("activate_components_on_start", activate_components_on_start)) + get_parameter("activate_components_on_start", activate_components_on_start); + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + if (!activate_components_on_start.empty()) { - RCLCPP_WARN_STREAM( + RCLCPP_WARN( get_logger(), - "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " - "hardware_spawner instead."); + "Parameter 'activate_components_on_start' is deprecated. " + "Components are activated per default. Don't use this parameters in combination with the new " + "'hardware_interface_state_after_start' parameter structure."); rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); for (const auto & component : activate_components_on_start) @@ -276,15 +331,16 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript resource_manager_->set_component_state(component, active_state); } } - // if both parameter are empty or non-existing preserve behavior where all components are - // activated per default - if (configure_components_on_start.empty() && activate_components_on_start.empty()) + // END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023) + else { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Automatic activation of all hardware components will not be supported in the " - "future anymore. Use hardware_spawner instead."); - resource_manager_->activate_all_components(); + // 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); + resource_manager_->set_component_state(component, active_state); + } } } diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 273e09d6a2..0fc7a2f27e 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -35,6 +35,7 @@ using hardware_interface::lifecycle_state_names::ACTIVE; using hardware_interface::lifecycle_state_names::FINALIZED; using hardware_interface::lifecycle_state_names::INACTIVE; using hardware_interface::lifecycle_state_names::UNCONFIGURED; +using hardware_interface::lifecycle_state_names::UNKNOWN; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -69,9 +70,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_->set_parameter( rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( - "activate_components_on_start", std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + "hardware_components_initial_state.unconfigured", + std::vector({TEST_SYSTEM_HARDWARE_NAME}))); cm_->set_parameter(rclcpp::Parameter( - "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + "hardware_components_initial_state.inactive", + std::vector({TEST_SENSOR_HARDWARE_NAME}))); std::string robot_description = ""; cm_->get_parameter("robot_description", robot_description); @@ -199,36 +202,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs } }; -class TestControllerManagerHWManagementSrvsWithoutParams -: public TestControllerManagerHWManagementSrvs -{ -public: - void SetUp() override - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - run_updater_ = false; - - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - - cm_->init_resource_manager(robot_description); - - SetUpSrvsCMExecutor(); - } -}; - TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) { // Default status after start: @@ -386,6 +359,36 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp })); } +class TestControllerManagerHWManagementSrvsWithoutParams +: public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + run_updater_ = false; + + // TODO(destogl): separate this to init_tests method where parameter can be set for each test + // separately + cm_->set_parameter( + rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + cm_->init_resource_manager(robot_description); + + SetUpSrvsCMExecutor(); + } +}; + TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware) { // "configure_components_on_start" and "activate_components_on_start" are not set (empty) @@ -409,3 +412,62 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } + +// BEGIN: Remove at the end of 2023 +class TestControllerManagerHWManagementSrvsOldParameters +: public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + 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_->set_parameter(rclcpp::Parameter( + "activate_components_on_start", std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + cm_->set_parameter(rclcpp::Parameter( + "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + cm_->init_resource_manager(robot_description); + + SetUpSrvsCMExecutor(); + } +}; + +TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_components) +{ + // Default status after start: + // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read + + list_hardware_components_and_check( + // actuator, sensor, system + std::vector( + {LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, + LFC_STATE::PRIMARY_STATE_UNCONFIGURED}), + std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), + std::vector>>({ + // is available + {{true, true}, {true, true, true}}, // actuator + {{}, {true}}, // sensor + {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system + }), + std::vector>>({ + // is claimed + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor + {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system + })); +} +// END: Remove at the end of 2023 diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 4ea3ae9a5f..92bde14817 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -66,8 +66,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] validate_interfaces boolean argument indicating whether the exported * interfaces ought to be validated. Defaults to true. * \param[in] activate_all boolean argument indicating if all resources should be immediately - * activated. Currently used only in tests. In typical applications use parameters - * "autostart_components" and "autoconfigure_components" instead. + * activated. Currently used only in tests. */ explicit ResourceManager( const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, @@ -374,7 +373,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Reads from all active hardware components. * * Part of the real-time critical update loop. - * It is realtime-safe if used hadware interfaces are implemented adequately. + * It is realtime-safe if used hardware interfaces are implemented adequately. */ HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -383,18 +382,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Writes to all active hardware components. * * Part of the real-time critical update loop. - * It is realtime-safe if used hadware interfaces are implemented adequately. + * It is realtime-safe if used hardware interfaces are implemented adequately. */ HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period); - /// Activates all available hardware components in the system. - /** - * All available hardware components int the ros2_control framework are activated. - * This is used to preserve default behavior from previous versions where all hardware components - * are activated per default. - */ - void activate_all_components(); - /// Checks whether a command interface is registered under the given key. /** * \param[in] key string identifying the interface to check. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index f32d24f890..96c87f5806 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1356,25 +1356,4 @@ void ResourceManager::validate_storage( // END: private methods -// Temporary method to keep old interface and reduce framework changes in the PRs -void ResourceManager::activate_all_components() -{ - using lifecycle_msgs::msg::State; - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - - for (auto & component : resource_storage_->actuators_) - { - set_component_state(component.get_name(), active_state); - } - for (auto & component : resource_storage_->sensors_) - { - set_component_state(component.get_name(), active_state); - } - for (auto & component : resource_storage_->systems_) - { - set_component_state(component.get_name(), active_state); - } -} - } // namespace hardware_interface From f7664086781cc7cf06066a1e710a8448f85e3256 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 23 Jun 2023 21:24:03 +0100 Subject: [PATCH 45/50] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 6 ++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 38 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index d938aa8234..07f167b8fa 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- * Add -Wconversion flag to protect future developments (`#1053 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 3a3447e908..a4a15f9f79 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index d0dd4d6233..0652ca745c 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 8acb46a46e..be94f14cce 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index c0e9c1cc43..25aa8e70e0 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- * Add -Wconversion flag to protect future developments (`#1053 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index fd2b295eb9..03643f091d 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 1ae3a3c626..c72e5715aa 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- * [URDF Parser] Allow empty urdf tag, e.g., parameter (`#1017 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 6724a9b175..130a20df7d 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 30df32e8fd..073e787c51 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index cdc7b97801..8e9e8ffd46 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-06-14) ------------------- * Add -Wconversion flag to protect future developments (`#1053 `_) From cc1667b4f4ccbf6152d435c7f2bb04998b8b3b74 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 23 Jun 2023 21:24:53 +0100 Subject: [PATCH 46/50] 3.15.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 07f167b8fa..475ecc1b32 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index aedd5cf2f5..9f6d3c5cb6 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.14.0 + 3.15.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index a4a15f9f79..31bec20a0c 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c08f12cb16..4e885f59cf 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.14.0 + 3.15.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 0652ca745c..7299338c57 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 186ecab6c4..2de7f5ee2f 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.14.0 + 3.15.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index be94f14cce..1a2d480216 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 54618f05c9..16d86dc985 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.14.0 + 3.15.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 25aa8e70e0..00f6f6f868 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 0a4990667e..a532d59fb4 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.14.0 + 3.15.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 03643f091d..e31c44e4eb 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index a45fbc7c99..9f7b7f8e4e 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.14.0 + 3.15.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index c72e5715aa..d5ae3c2304 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 8a3c6abf22..67e7be2e53 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.14.0 + 3.15.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 130a20df7d..06dd3441d7 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- * Improve list hardware components output and code for better readability. (`#1060 `_) * Contributors: Dr. Denis diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index f8b8caa615..8fe8ddfa90 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.14.0 + 3.15.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 6f6a49199a..d003a71365 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.14.0", + version="3.15.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 073e787c51..cdf03f67f7 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 0bb93f0838..b5e82f3a99 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.14.0 + 3.15.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 1dd0977c7a..7096280cd0 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.14.0", + version="3.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 8e9e8ffd46..ec7e028687 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-06-23) +------------------- 3.14.0 (2023-06-14) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index bce59f3a4a..ac00e7afd5 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.14.0 + 3.15.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 87c91990c870e55e8956f7811fb6d7c237373dd9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 1 Jul 2023 18:57:57 +0200 Subject: [PATCH 47/50] Add CI workflow to check docs (#1069) --- .github/workflows/ci-check-docs.yml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 .github/workflows/ci-check-docs.yml diff --git a/.github/workflows/ci-check-docs.yml b/.github/workflows/ci-check-docs.yml new file mode 100644 index 0000000000..90a822aa72 --- /dev/null +++ b/.github/workflows/ci-check-docs.yml @@ -0,0 +1,12 @@ +name: Check Docs + +on: + workflow_dispatch: + pull_request: + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master + with: + ROS2_CONTROL_PR: ${{ github.ref }} From 185fbeafc97334eb9b09fde74e70b9eed30616d2 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sun, 2 Jul 2023 19:00:03 +0200 Subject: [PATCH 48/50] Create doc file for chained controllers (#985) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- .../doc/controller_chaining.rst | 119 ++++++++++++++++++ .../doc/images/chaining_example2.png | Bin 0 -> 37092 bytes doc/index.rst | 6 +- 3 files changed, 122 insertions(+), 3 deletions(-) create mode 100644 controller_manager/doc/controller_chaining.rst create mode 100644 controller_manager/doc/images/chaining_example2.png diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst new file mode 100644 index 0000000000..437cb70509 --- /dev/null +++ b/controller_manager/doc/controller_chaining.rst @@ -0,0 +1,119 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/doc/controller_chaining.rst + +.. _controller_chaining: + +Controller Chaining / Cascade Control +====================================== + +This document proposes a minimal-viable-implementation of serial controller chaining as described in `Chaining Controllers design document `__. +Cascade control is a specific type of controller chaining. + + +Scope of the Document and Background Knowledge +------------------------------------------------------- + +This approach focuses only on serial chaining of controllers and tries to reuse existing mechanisms for it. +It focuses on `inputs and outputs of a controller `__ and their management in the controller manager. +The concept of `controller groups `__ will be introduced only for clarity reasons, and its only meaning is that controllers in that group can be updated in arbitrary order. +This doesn't mean that the controller groups as described `in the controller chaining document `__ will not be introduced and used in the future. +Nevertheless, the author is convinced that this would add only unnecessary complexity at this stage, although in the long term they *could* provide clearer structure and interfaces. + +Motivation, Purpose and Use +--------------------------------- + +To describe the intent of this document, lets focus on the simple yet sufficient example `Example 2 from 'controllers_chaining' design docs `__: + +.. image:: images/chaining_example2.png + :alt: Example2 + + +In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers. +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: + + 1. When a robot is started, we want to check if motor velocity control is working properly and therefore only PID controllers are activated. + At this stage we can control the input of PID controller also externally using topics. + However, these controllers also provide virtual interfaces, so we can chain them. + 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. + 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. + + +Implementation +-------------- + +A Controller Base-Class: ChainableController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +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. +Therefore, do not try to implement any exclusive combinations of input 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 modes 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. + +Inner Resource Management +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +After configuring a chainable controller, controller manager calls ``input_interface_configuration`` method and takes ownership over controller's input interfaces. +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``). + +Activation and Deactivation Chained Controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Controller Manager has an additional parameter that describes how controllers are chained. +In the first version, the parameter-structure would have some semantic meaning embedded into it, as follows: + + +.. code-block:: yaml + + controller_manager: + ros__parameters: + chained_controllers: + + - parallel_group_1: + - controller1_1 + - controller1_2 + + - parallel_group_2: + - controller2_1 + + - parallel_group_3: + - controller3_1 + - controller3_2 + - controller3_3 + + ... + + - parallel_group_N: + - controllerN_1 + - ... + - controllerN_M + + + +This structure is motivated by ``filter_chain`` structure from `ros/filters repository `__, see `this file for implementation `__. + +This structure is stored internally by controller manager into an ordered map (``std::map>``) with group name as key. +When a controller should be deactivated, the controller manager deactivates all the controllers in the preceding groups first. +All other controllers from the group stay active, as well as all controllers in the following groups. +NOTE: In the future this could be done more intelligently, i.e., deactivate only controllers in the preceding groups that actually precede the controller that should be deactivated. + +On the other hand, the controller should be manually activated in the reverse order, i.e., from the those closer to the hardware toward those preceding them. + + +Debugging outputs +---------------------------- + +Flag ``unavailable`` on 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 irelevant for the usage. + + +Closing remarks +---------------------------- + +- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. diff --git a/controller_manager/doc/images/chaining_example2.png b/controller_manager/doc/images/chaining_example2.png new file mode 100644 index 0000000000000000000000000000000000000000..1ba49a116edcba1f365d76d4b52b3464f316a118 GIT binary patch literal 37092 zcmeFZbySqy_cjhANU5Y^&<#p=4?_(-bc4Xq-9srLC?$D5bO@A>9pv z!0*iSJjUnqe820r-uLf!EtZVTJ?B2>-23c(?Q36q!V#+Sm+^1nV_;xhR)EWBU|?Lx z#=yX|#JvdqvVT7`3j^a~v4^alha<|$*1-~kkyrZfSByMdHqP!IjJz_8JUp@vmKGjv z&Q9PZ_`9Ps($>n>68ZOgJX}27>|Ffp+}v8+f{eUU;1XUbK7I~iAufZzuQ#){boyr? zehw}$0E-?sHzO|$yi&Gx@!BW@KT*=mhkw=c7SCE5O0R0gq zxR#1CBabwA?O3r^wfVZM2?#Wyo0`0)6B}u&GwI*p)d5ZbaS@_3;t{QKZixXVd-IJ{r6vaHFWv8 zZA?9NZ3LBtJREd=RqVlV=xaQDTrB^-25ISKYXRl~_CywJEz;J^+Re-nTw?pr?nw!H zYTChUEKLQ3e5`G_+#P&Sa(`bgsNN0*TYCd1^JOwn{t*4LD50+uc)5Ud`6Z-GfJ(%iB!| z=3*h>#BZhNA!{ioWPnn36?Acy@ zS6Ocauco`Oh6VVbKEh32Q(8sQM@iiSrD`sxZ4Ne9Lta_fSr{qhF7GU_A?sjkj^I~R zG7vD3mG;q+cGLECwJ;OZ5tc*Qx!5SXJ1XhwTflkUot<2z(%kyo+-}^qGIp*)=KS_%C_YzvZAEDXML}zr9j~C7 zy*j^(y{)>I9+FSi*VR=3rDA8{u4$)e;A3sA=VYa&%EPBEqpIMgC8OiUXXRs|DW@l8 zDeUTvKqz^@E#(j}YXtg6_Ez9uq=U5&+}%vbMqZJRTThnX%0*Ab#aY?c)>g;dOW4ZZ z(q7)$Lf*hrk6Ybb&Qn=NiqG3dK_7h3QXc%HC?lhy>S8CYY=_Wvw^Y{AcJkrkhjV+m zc%!UkWt`NdEd_yv3hIGl3)65BfN6T6bS-VPrEJ`lT)9=PtvzHdcAqJSd!Ezig0>)`Ir z&#xtHZ;fG)3xNcmK`O)DwYY@Az}CL-CIycOGVCE24Sti!*AhZ@5OKC#0xWU=Mof9 zlNWY!vEf1@T?92;oCT2r(%eoiFbkxh8$tlZ>#gVPqvYveV6G+OE1<8Vreq++XT@!1 z<>rn;S;-1L^IpE$oN@J{ks7MX9Q& zTd1IPP`*ko_OkNkJo=uzz^vpfEK%0(^6paLA|wbXFf&(vFC^04O4v&VrR-Fputd(k=55y?aZUXNvB`Gl3H z=o}#dO{CMnlEIXfL5a-|B;`UBXRD%{DrX6?DIX_l zJQqamYv@w^#+|c7Yo;bo))E%J*83fXgkr%1|NG|>Q|v>?;N2F7%>RA|4kgaUn1|6| zlKj^XS11N{0qfH-JiJ(ppFe@bSXZdTSlT1WlaG#2Wj)_^YupzVSBFbZZ+4UzzRC05 zUvWFyG8=nk9W&RJ@V@N0?kN_Wh7}VZQHuNL`0q)!v;&={8jO$kSD8N~a~}GX;^E;{ zdT*K(YQBh0mk2OuX=(8<{Xn1)h^4^<#q3U!f-pdT{!HsZHXt60y%{?zFUu4Ym{gFH zwVrMZA8*_cbV}DM)R3jR|HHM$Z9YQmbnkPe@4lUWB{G~O&wFDcuAqP=MZht2t|v{m zBkBhIJMYbFxWnVs_8)?=3Fg|uNq0sqV!HYy&+pvUDUQvNexzt*lumX>4`J1l%7~3m zMl`qoWBLIh@?dQYGBf_lT5Wmq-J98nRafIwK3lD{C3@+Hmw4<3SyK3H<0tC5UTNgY;4x|_C9yF+laJ9E`IseXYAlJ`x;s{7n?UkF)N3tvp(l;txW@l` zrPzHxS-4T-wVs+fP-^@x&-U|k#km@S{uEy8?sQ>y9S4iO-5eGy(C}kNNx#M|zwTh%UeTlB^x(_NtED9yJVL_c-V9Nt z3iI|n=c$I3!>ziO-5QT&jjm+QtV`4qMg~@OfXQwaqZohME7@ELHF&~Y`Bvk4njg#0cSkp~G7v`IBsFl+%%d9p8oFpP9-?~2YKiwZ$;q}Ih zK;Pxg{NP(Y}5LaE{*1cRBF+pHVm6ji_U0oo+thF0rV>NB4 zQ_d9ArwZ6pPa|G%fkRCpWZB0%Nc-Pr3xzO|^wU8r)JS2wFS72{I_G_RAEO&gJc6Dd z>dGwv<G_04>l4ne0izM8KYI6wKsKs_GV|4 z!{}|ljPF_m)<^JI>?GU|;6D!tI4Rkrn5Nh)@pzA~bHmo##qOWgC2|-Wte?oZ<@BTp zzQlrBypO)>3?a*EKB=)8pqoFHS-R8WO{>>1C@KepfR^axJDpDU+enKe%Z9A@C3Tha#I1t<09$Km;j-~NZD6Yq!( z8i&HY;X-?5Cd0$BS$38hSDJyk$e_AN7Zaa5Ts$8xmneDv-c^Y&#rnL<{W%X5jri_1 zVK5@&h{^R`PWG0cEw$Ql0m`};+p0u~EY1=8d6duo6CNSZ-BMBu|?{OKRS)%?esO$^`( zG6h)){`myauZdQ`SkeL5E@%J!&Rr7V=@Ou?ZvE-&&T$8ew3UWSjbE}|rVv6%{MeG- zKNsR9J-@O)bt0djn-D0DiwGQVy+VQghuK1TAhw_K)a6slf%nBC#KiBPu3)r={eHqy zvKX1F$s@c4OxLE7os7`HVkfkpRoZ^#{^#9Ewqh{EThF#&1V>wG3nhSet&pc2C}y&JYYw`i-!UR z*nRbn$uZnj0-N`mj{rv&f(hcwWnAjpe?F4yvVl3zSrZt^pXpG1;eP;5?Ty4SoZt5b zYhIEG%rwh@R^CtoyJHZXkrDsrBMlS)nz;en7r=v#->|1g|ACTL&zSDCTQ6fjaxUQffHpNG>x!Nf5kNKYlO;|38~8GmjI ziVi(`S1iHijsG|AzmDVo+wcF`PNtm;Yyk+d?8_972hKAF;XGN(IP_9Cx%Cm@OZ#L_ zQ;zj7Zw|X~-!@1+xahVsr;t^uww_BV>IL6j=$W5xeo$zfI_G}B=4eQWlL_+yu*I8~ zp7#7{i#GJrWiR^3F>Cz}oqmF{PtR3u17wo)%DV5E5Bd%lk#+GyB)sW4Fmy+|>sKOw zp95G1lm_Af-14U~Q%Qgkqh(}eyQ*wIGq^8&)M)Vc5q6yo5jz;Q&?vtvHIK=zUztb7 zZlHCO-;Ny{rVohcN&Sz*!eT2d-YX`tY0JFC;Ts&m zUfgs5r?B@mwY=IN4ppgX3Za>d!)ILUfks^VZ=5m`*|ZhqVyM{BknH>JA~SISIj4!% zOfBw*42gi++KVl2U+a8$+~(l6qvbi5fOp61pqGzknZd&pwd^Ld{qnj54GOG2KFwKS z*DHq}A8vBq5^zA!k>*ra+YQB2iTlCj23PB@X#NQ06$Ccf0@TUQH4e7}P= zRxJ; zQ}j)KMLaE(o;nU~Bg<7bEku^(hbJlt82cl<6l5Lq48adEuxOQ;(E(J0Q!Z zwe?c{!}+eMLG5wTHlTfdT}|@TV~Sz&(74tQ>N;azR{tD}sm1w?!x&wOL3R8!c7p;U zlaOI1HQ3yl^BSe>Tjw&9`uM%@sG6fwaE$7s+o|o3KD1}O_F>1nt^(Y=O^ZX;Z}Wyj zNipwBs~A)D?=62;TK<$j^i`SRR!f6Y$Y*oT7w<4a0jt%U9-K8ZGsEM)pgf->$uE_`~LAQX7I%8 zId|{0IL6I;ff=sAuOT~RUmJ$W;*LaIybzhd~PWlS+;YH=N)Q^e7S8l{;*)O&hN2&g$RX} zC#=@^DBhZ4``~h?VMT0C2&*@*?H)hB4CS^DnX76!n;kAq}uQL1e zT#nLKuQYeT%fHYj)Os}8b^9jC7CI~EOhjG&V+<&QSjhjN%D>j7Aw8ynBz8;r3O0<- z`!&UNz7TaA6cN6o4e&y&=8Qs#!LE-ZJMUWded(gXlM4qro&J>E|6JGkc?{%fa2gn?k`PmXoo>{tH$w~PYmbkAei2R z@w#-1Keh#fS$42 zq-(N0{_)56-F)?Da`7OYgT8b3IEBJg$itmFLhL2J58=!)yh{)LVn+wj~Dklh`o3-?8WBe z@T{)z$VJBwWlawVyU30G#=ZP0uH%7>1Tx4{>5%+$D4IMx<#ZwShU1mnL1FlWgybj* zzrzi7f@^nT$xTpFAg0rT$d#=B%1UA3V+x~1M--3uhLPvw7w5*3kd+bB`|Mv{J0=j* z%N6vswzMEWENaglIE025P3j&b}n zF({n0Gl4l~ZK~1Khg-MIgdGqaTCi8Zu~%BWztVoO1%%Mvl~*S9z6v?8&_Oj_W87Z+ zt8AV3&vvQ9qoTARo`B5Z0T=+>6<)Bs+j_?6O}Y`0S3z6kM`zpmsXO@W0smzVB92rM zBqK?G9JqZn*BIH)+Xsr{{gb)cuS`#`|CmoU%>(GK`Kt+;FY>^1d4E~Yg9V{Rf~Ojppx z1l)QNy|RGpPqVnI2GJ2OrxtdTdBSeU^sVD@0-A;cNRkwLY3lwtJHP#KBJkcy(G) zBrYJJgQfs4T)c$vgG!$6X$t%7YA!O^r%b)U<$Qhvk8x*zK0OT_F zEk^Q*|M#7)gY_>%Uk3o2I0k4VG7clX@Ucj8PDSv@tAkwhN`Ub`@FBFJF`{^lB{V?; z)_5~SZa(ps^Nc6qG7QZ14(m(0tqY0|Av+GL86w)D!(jS4Zi~GoEjF}rWu}cBfW(aV zS|9H+bgs`QA)GNlcAuV|Y|bwYJcBd`^?v~di3Zh)ESEz`!X}?$3zF&JE-uM{Fs)j3 z^Zl=2I87b|4T&+GDB`VSwIp93FfG>=O4AyR{fCm0+U|0;mjZ0%cV0-+_JMGuQBV*o7&V7EN z#Ei|^);X#sp?=~|Z16aswKN#9E_rs;`pu`zsP8o|E6yAa2nv#Sw>5r+1#6P&3-5T> zuPqh$seU)Qk|j!M!&^HN)19GKBpz!h;<>^wQfjR9>E3IHF(fcgjXIQT%&D9BsuPKT z4ILPa)a=#mUxV4>ZUs%=+z2|jX;Z-3;_{-zgstL@6SfH1{g4_lic^ty9_|QHUJ{MB*Fp5RV2AYc}IkFNugNjfKndp8vZu=%dGwBvLD567t)*L-NqDOd+o4#AK8(5 z_4QV8%gJ}UlH*V1M(qsgG&~@e;XFCqOn$DC)(xC4sidU;Q8~%8BWK_y!(n9RRfrr| z?T(cN!`+Kumv~uT5&qgGCA=kC?o<`imQ^ zS7uj+UINU1RoHtY-Kf@swcXy_+&qzpQo{djx`?MD9zwTZ0T4HQQz3efJE5&tE@UXVfZbW%^Uy3dDUJ6O)WX?cZx$@l9UXVcjRb>x8Olwhi$vx#`EJ60Uaz zr%oGm7Mzb&%$-x1VXeHDiR8Dnlb>kYuULf)zNHBnsdgrl%tFnE%WT@D@K}1>!ns9< z$qr^yjj*8nHKsg(nx2U@BslgVlQgjVqg!dLnFy4`xw@?Bc42!wC`G`@(4@Kw*|YtX zq0gTvR%4-1G_GiQ-~9_$r~)t+>)KM6AtCo{?V-y=MdV^+91i1n%MG%x>2Mlu zj8;ffZbJH&K%ppM?N(15{Uejfa!Qgd5J(dvAN+g>G)s8b8Ii$M>FuP-?1OxmC>BF{&pfLcCRA|YVKCIg{L2Xf{3{dQz1DUXTZiuS!@QN7wUGm$ z?!oLUZ$DTtvq|J54hRaj%fA3o2nQxT)u?o`l}j&$Sxuy%b3AEsuTpI}S53G>O{u%| z_UEgmky$LZGCbCuEZcZ^8Z?X2pYmXgpIaPz^!hsUR!Q=h)iRYyif&O+QKd%)zTyeR zkakgN{;2xvtQIre?YQtRX_sKQVIQA=|54w+6A4}OJrHt&u_TBxgy||3XT-kGg~QY} zn2N&YB0estkz<+ro(B*$oANjXy@$V9;?yYH1lb63tcEo4$!NZdBlKE{;XfGyP00wh{tKsYXxJNjOJR0^ea6m){NA+nh3;ZjTHL zL*&%_q+DKE^$a%BLKl0|m^ELh@=sn^Y1^*@_mq*s|IBYU2tNf_OFV4lKT=Lv!lx;eQIhH;BqR*~6r2ok5!MF0^Xgf`?s9nk$ut2ADb^(Q&lnXGxg3kEO8GxC zS++(4I?jgdO8C#?480HWe><2}c!NFFR?r)Byik+5IkO$W#z{dH(U9rX=C2VFG<(J{ zGD&)RdeU|q5OXe3#;4f#^oNhux#AvLM;O!``!hCPx2+zg))MrG5|DRi zOFhu)bQ~_$x7?Wn8KCqR0Bl`Z7`92x2g|Ne-`TzulFbK~g~7B~QjsVs6clq$&_8EUBSlryBy137d5 zN*;e^Sxom&`hIEwNL(Q32{bDn#>9EKKc6h7*grcxA#GpUY&_3IPA#<(O0K!u7}+oA zJ%$WY^sfD?>NlB#8B~p8+7w`RN@8~FI~^>wE@oEI@kN#t3HW@!>RkG4=>%I1Ih)j z*+C(z3urDHcceoFZQ+y^(u4)ua=94%ncRLqcafw#unF+KjDTTdyI=)sqjLbIuS^Lw z(1L{HR1}bD*P^0~kENk~h|eH9fcK>G=YwM&Er=HC< z*~G-eJYH*hg&O%0*XJ>Z5+WrJa2M>CwFTbSl4UQrSaJS3*w=`bM#{2CaKRZ?Sfezb z$?*OfH`Dsakc-Z1nG;i7wi&R9mZ!)2ih8|if@*Tn6s({upBQujhpibk*GbGM1|$-k zn$MNvUORs22EqzUkhm0m1(F4H22^R*dXbR_9RPm%xBrb&_MB{ffODE5QYM%Zm}%cL zmm^vvijR;_iG1*joY{JWQJ0O*I)JBV)&X4lGo!aIL0{GGLk&rM-A<6}2`oe1^^oq;AUlAK_wmkSb64S?#h!lNscd3qJ*p8&AhAB$&HLJk(F zD`tb|r7@(CCQx!*3=73ydF3!xsUB$X_TGQw*QXrvXCuLSm1O)(vvmf6{G2j2@k{{wC>T86YEte$C@e!vf&j;!L%iGIhP1I-a z-7`R9-SRvcw=ZK23g7wy)(i((ZJuo8wctE$+!B?{kj~SoQBi{;E!Jf~AIIOw0BVEN zGZ4TIdj<={v&oc>IoWuIvRJ|B%74U?9^57Z{Gg!XHxnneyx*RRrGx0r*2mD|{62=d z?BE_yKImo!9O&N=LzU}d*1F8V(4~056w8IWSlnl+TY;N|9DEHpH1_#EpR64wh^C4U z?%k}wNiG%p4@u%@5Hx7pb|n}OWmHaVe&3;HhaJeKKfs867AOGWl0 zr5}USQiB}#gu&W6?&6mI5iGH;Vv0e_pGwmV_A%zMa9L+mkSmDYetIVg)LrfFn7&s1 zC+U|`2+VDwcookp8zg-DaeZK*`r(%cgc1euy0o-ORE&VZ=+@lK++)l45;tBPKXDiu zfRbhV7DHbDq=RYYnhJ2LiCUkocy#H@Y!Xdq<%rHf+ye0 zK9An{etI}H)R^VBA)J)GvOi*~+x4>HXqEubGEs^Tha2zT_q^O2FB@M4ykWt!KwyDP zfG^R`x=bmem7y8wIs|e%aB`o;oA~TFl_w>kwD? z9Vma;F7Q=Kwvt9Pxa<-2D#=Oh6n{+N{SZb>FLpqE<3?czkYG>~@IW6udURrW2_Mnu zvTgp$SK}jghXWf4+ZG;2iR-I?rQ9d5F^dS%L07RL*!b{J{Huf6G*DVHSGhG1bF?ej za2vp}E<|eQD<$htbpjX8s-7ECZ1_esTpGC|X9y;i3;-!hT*R{=myJ6-tkP=-O9S+> znD~869l?2e=qg-FCaoO0kfd8`q`o)_n`qekq!j+1Pzovt5@Unc4u$bmIb>{F@O?t{ z^}=%ih1jGBFpC(e0lYf1PiFt8x)1Fq?cd41wuiX%lt$Q)G}v#o4ZEhw>z&cD=L@4L zAGlLi{T)|sK^c;bh>-lGJz{+Zi%g36{jQeyR~-rQuNE}p3!CnX!>z79oL*5g_;7lF zR)_%-DIa9*pMbuE)uit2v^SyHR$!I=h-$caNNp(y2i;HDUtY|hm%lPr>-mx-6Vxc< zK)I`M>U~>VfiF4}|5Jj*i&B(-d^0~9L$T`uevTn<|81{q1}=)F5XBR{>)eYvh|3uZ zAY_qQo^6kue@}fs(ecY`R&IPV{GjE+`yX*U`?H?FFpENOuKt5`pkaS4dA5^JB`dw> z@}UI3eLh866~de_J&|v@WzOlNO3h-sTPJpg0NtyJNVV9l-0$;o9DQA_j;pqn{I~Vh zpkSRtx~5%Vmia)a*6oKE!_*rT3K#QOiuKWu|pIJprPlco0;W#v*;z$N9(J zIA?jR3@Qu-9g<>GsiL%KsWeHlw-OtG49snBX+S0x_wng!Sz{uoW|7UjVcCG}fjF!2 z*2PTN@1R)39QTxF?+qx~QJuU$5_Fnii4i||%JfV<(5|IJ@t?>?G>%mzRuC${?f8n> z+8t{t+*WCRIyS|PlS67Y*P{A{lFTRhhf1k?W*SJSC~JR3+=7?hz)gt3hG58Bx?&K{ ziZnpAPNvKZa5#A&Mr#0E9s?nRxPz)Igf_Jf8c28N4FKzh0Z-yFLTzN9vbQne*JS`s zR%I_xz@{I%KBCM4-QEPJx9Hy2`{w~E;(o6|nwFr5;kP2$pQlKcJzVU3@RO2`X3CzI z>KPz?ZL>A!;DGD2*DGDQ_*iXjp~5|$h;nu4%knBopf2AX9wQ#|ZuWeM`-XH`ch>B+ z8%r&4OHJP*K~Q32C4w0OWN~}`LvmvoX362Lx=f%xBxb;DIqL*5um~}NmS0Wz?F7PT z%q3aGUTiVV`Z7@76!j{;1FC_H9I#II(ljxj%5mL@JULdp{jzuk1(2U52z}ia5wj)t@e$|4RAs-Cq5h!2VH|$S+AD=jonu1o^b=lc zhicft1BOpahKb*fv+w(GO1HSx2>f`X+~EIR>zV=5H8dds zGO9JRe{@t&|D9Kf-ADpPdrfxfowwk5BVeTA5z{KNQt-8?8@yPvB;ir9cX8df60ym{ zBd5FCxNkqbMM5k!(Q<1=C6EqzeH~T~q@zN_#V^iSp{J&P#Xo&F0}%~o385XP*Dbf9 zuCeP0)KuR~D9+~m6vAq3m2sR(9?Bx06udEnYh~%w9<*1_NUAbGV7b|8g6H;!;LdCV zc$^F&Hdzt@i6{&1Cm@f1JpW<%AGc1!g10p@0wT++GupRAS@IHT%-aKkAGveG@*+Rd z#T+=4zQnb}by`_y{hloJjpT&S_`CWzHRDbpmKPMkUCBr^j7v!~y zvHL&A#==8fPXVna!&KK&;zNnN-j!`KXo_WZojhW$YLhp520yj3^_5ZU!2hNRejY_4l~Fl|%R40aFg+XH%wO=9)H@h=h-l>flyQDE0` z!dA4_r&uf`R~(BEo(MnO{iHPS`R$UDGV=SJ2L(p-g>5DdT0VDB=a(_6ja2y|d7xFB z43w=!dC)h`Q`acegV(CjO2b9V+^u_`2Leq}0iOPy83bk1HR15Pe^lr_4xM%?IDX!? zCgK2Zp*7XLKtd{;7#+{H`u@bA{hOyMVRO5LvNL2CROLjZz+)-gJ4Iq9KJ9WTqr80wAY|G8I4ye+97iq8qJB+^}b7lQ~J6OS-@j|Qz7MhS0n z$ey1XG%bdV2RItN^Xhro1ak2R8U}%ykx!>nPoHI4t_&8gq$@Gd!;Tzx%)OucR+d+? z<0Eb_zjFR3O@V6U_uXKvj`qLRKUZvWOQ^Or z4QCg4Nih59({jU#)WS*Bb$%**#Iw#yM(x<@B6oN6ewH{62j9V=ZLnkrjum@ z{$={s{L!smOhyJC5v)THqp7f&Laae1T92=ses#GDESw@z{mVlSL%qzW%EEH;t0 z-rZc>H-6N^{gk>M89=94^n>ci1R+_gTu0W0s`KI8zERogOAzxoDr)dkb;;`1J>K_X z|B*%zX<;2@TSW_Z33Flkg0QHsTl#9E-t#@hgYN@M1qBi(}TH`h#m4)UnS|R$sQOOK&y7pr1sk^zvUe}^ZU;Q zHyPv4LqbFI1=?ay(ht*wnO^*t4Q0Q7Fpj2j&d*MRKPGl5{Z=QPF;M#)*PqC4ooSVs z7+;~5C?3~q@UN+Io-!f5zh?0TsF=xEHDojcOc&=1UB!h~dA2Zaq62lM6i6x{HjMK{ zXx;OBI|j+k0nhzXRr>igZ+nM0f0pl;Vxw5pJ{at@4N4rUY=D5v>dR#eA(emH*3qE| zpz9`SDp7LR=B>$Q9NP!YE{(#?e4sW`PV)YzR91i z29$(Or2usY;0L?g{)+Yu$$NE!#rm3n=|C&eL9VJ9iVYGV19a(%DE_Nhw$hV3>Skd= z8?+!ZLL*Kh1`vA4Da_A-lD*LV;!MD8Ag;P&_*!}0Pg)qYm5nax89(||VkkH?1*nG7 zv&R=!X81$B9s9GTcG}5JJ3$IU5_+Xnq40i~aU!U_b%T5(5nf>Y?sY0*FXdkQgVhFv z|GY!n>@iTn5Bh|=y1JspOUvWcFUCXfA*J(Z4#xiU zLXBG3Wg41`kpUF>z8D}%)DD)Jnlb?idK*g}EECHH=;ZTc!bvKfCPi4aixO$bNMSZ0 zOH{nX@BGy$Tsed%_j1TQ6Xz1CCynB5t%7WchZv!Vj~SwG@Ci8#stQOn0lL(*;4rSO zA{iKcum~U?amytLI~^VDF?LIdd<<1IaW>#xU;bD%J&y;RXAz^vCJeX#3Z+Qr?Xo(zDgOJX`^oZ{ z$DnfA4SE5RpdLvemF)s$-@$uordTXkgCI^MK_4=cQK=vaWsm5^yr={A;_Rlx@#0pe zK)E>K{h6reO5RG-NqDe$#8v^UUxX2h971Md@&jPbN0)Bonp_X0UI>Y`!$p)gM_vrm z3Uf}z2mny-swq9ph5I#PKadAY_q@!+v~gr_Q@F|?592bspZn{7urnwS7wM2Bv~P&A;WV0nqLCqFsY~g!Tj{wF(ccg9OuUf0JG1t<95DY1=L!Iw z?M$D2uuu7iRUdq*+xHA3v}tcp5{soJ*l&Nub80l54oVyxiLebJmNtTE0c(b%yD19s z5Nj8DQg~nBH-|ReUnzW{C1+j&8T=C5P6gh0*y%kBRBvN+b7Q1awzbRw^*H?eRJlo= zgA}d*9bC}Qz??`SXu2Xq*A2u29`iO_PqB#Gc*HQF8OAhW_vd^7qn}cRTTb)ulLbc- zDXqN^vA*2(P{8?Xar{{nrKms`6#~~ZjP^&!BEL`22rqDUb}?+F(v@_`URl4RdS*fv ztjZT>6}lwhkwk8ht4wN{#VyfsgPp7pvUD1nxF$RZS=ZCp5L_4^KV}3u4$%Q>V7l^h zn+S>QvT=GQwqUNFl#kV}jEbKB|Ia!gRlp^q<)`{}sEQp8T0`~&qFU=_Ss@Owhld-J zq$na8FOm>?A8aFwld0qfg<-b_&Zkk9LxvhhR56OM%kU1bz>vk6~Sn z_Wq|PyJgxY8R3jS z(hWbd^A4lL3LW8_66<#rp(}w~^&RF{g7^-%YU6&9%z;=kiEpl)9K~M8#S0<2miD9h zBQ@rVBCgEaGt#Z3SLf`aFXU)0Ic2=1S?%_G^6t&Xs8fqAuVt*A;S2b4NR5M_z?Cqp zD9{gL$7lBXr^xmST_BbeF1M_lW=I`fTQvxEJF}6T=r}!ePsLPLYR5$n$E>^^5isu% z*?f~P3@WW6s1=}Dx+&t&zf>idwja(?xO0N)xs3HctC)iB(go&;f$pukZi7`e_$*Mz zQ11INB$;6(ZC?X2u5|7em-7`tS&(~Pa8`bC4V`>e%w$}^Z=4_R@4D{LK?2kYPwra3 z_~!`T<%DdZHUP6|S!@D``54w@D`qR_T%jQ z13oKSGkMo|l(Tp>>?pqrWOunbVui#cv4ia>YQ~tzE5=N1E!yg7*uMBJ04Zki(r-Ed zS&c1rNEo+FlFBjQ1JnhcRk*$7ZE6(sJ3w(#Bo}M>8t|<=*1Z&H2D=ZW3XIcHaq7sh zC1N{Y^KlK~t(jyJ=L?)RwVf`k$D1R0(D!nf>@e}SSbP{dOhFi+dT1B;UK00awr~Od zx_xv-=NLt=&OYqKKvu%G@r7!J;_Q$|cn=6l%=ju{DnA|tWNqZ%Z4M6RrJULKU(E0x z{2o&ZJ4v-Lb-Qwhp!8jmfY>g`6C@l8$;rO&G<+t7!u>1D=f6#3z?&4ogSxr2OBj67=1LKIUU%FC z9=+18?{DaclXRJG3NYybkdZAX#mE74yyBsha=#gZo5$P1uX(^|)hOwy54fIj{Jh#EE^CpritFfTW# zwnGc%ZvcYe$D-tU9MB6dJ$!@q@0Wmmt0-RyS$kC)0xiuV%8ZMPtKENDP0Ku{yGpT+ zh)I830g+jnQENyJd3tVz5~yt2y|M2_VBF5!@uAu*Z)o)O0D7fF3o2!psSk?!8y2od zruWP^XmhGomT^KnhKjTmX*fVd^9iSE=~-*zJ{t#SM8(^-1^O7VT;1K;H{TdeRwR5# zZ!uU0NV`m`JaU;3pOn1HaFJ&gbxrqOzQpF$exIz&C$~Uty^eS+X#;&Xnt5uh%l#Bu zp$Gi$_2wL|SikgjjSrZ_UIMj=dC*jr2r}1hkiH-R9%Z|s2C%TU+$iY4`vB@&v=tQ< zD|bEu@xs)jqwb{J8dWyxXw{a8DCs&G3j%s}cH)c{WJQ6dw^NBbca%XPfT2@h70_Dc zA~`}`4XV@2wo!8q#V-iM!w)>pWt^zQnehfHYH`(Z`^r0Ax_-c&Sohn%C=1Qa-HNpk zi~{83#i}0oDid(V-)A0C4}MPpWs7{R0)$lPCSBCEnb(om*yBNZp&4jUjd1*0maswp43XN5 zu^2){!K1m+&ImWcunv75b19i`xizmd&N&60}Pl z4B;W!dSd@F)pmsgPdr3e^VXINql7WGi88L-kHr{_(~WnVgP9r>W}hl8m24+#tLJ(% z4A*b5gC0~niGSpJw{=VNFskix#_LR(C*|dG_|VGRn)giXiK7>o8C7bzYHqoIeyL0L zwYQ2mW!}U;IE1{1GseTGH`e7#;%I7DJIj~K$$r4_7d0RJ&s&E>7Q6-ECkZie99Bjn z6@q~X?BUb3i5kt&ExIaipeguN@8=C*{D%N5Gu%?`+e2SZMV2Kh1r^Tay#LP$VJOGB z2;P=m>0=Dc%G!Li4Nfoq+u|!Ydk4kt{Xm&tGg{kk`<}c$UJVK#Xdyi?JlA-l3E+8e za|@fbV3najaf}nVE1fa6MnM0Ehliv4imM5$TxMD-9maTSP}{jHF_6KcB)iRf8E({L zY344`(f0I0>o?h6bso`T5+C|5h_jNh;OJU)4n=Je$+#YLEW4bS$I3z(d&SQ0l|{ zrhj>5jaQqX11dhGx3U949j6m22>>%f)04?7QAyA?;fcmTLY}SBg8MIj(y=nPN<=P|0jYjC(gPNEG7pUI8~8zcYZp99(d3%JI_b{g$LMn(DSAt!`%`U zAS9ujv{S@%())dYODpJYm6 zfEJHX8{kLkji==Lap9-nxu4y(vTU7y)K0)2I*;~fEWCUT&_w86P&yNLew zf@Tr8#MxlSTIwPA-UZD%Ld}<(>+UZm<%2#Zc>;blioR)+nO&1>j(cd{tF#gzIa$Z-mMSpnsp zO=pmbeEaro9u%(aoUF*vE3NsW9k>!eT|Zn^W-Z<960eRn|xjhl)Zfm5De z?*wWFKw);}DJ0|pY@5n{K4uNr`prg8RA-CyAQ0|n$b?WxlxE!tb*)7l-Zx(9+V%eL+e%0eS=w z)*C_c;UtVC;?HadO|<|=R>UzLWYa9>y*{3Qom#{LE^c$a)k?V2Ngsnk9<~s0GAu6w zu1A-!0fd{1bRZu%*qD4-r-m-4&SJQ~)PDKIj?nRZH|snRe3QbyC@KkPSCW@}nva^$ zZ4*0dulksLq4kf^Wq=*fRiTLHwLlk_9fclq5bDB(iZaQ_E3Z3_xHr+YOrVUunhY>x z7l>kLnY-PjnFP_wFRJe1CiT%ZK*<6+FMoWS&$^g?;`ZAq1mNMHnUtd2HX|s9$%dPN z>}ybD@WX|hO93I=o&e?aGy=L=SL{B)S-LZqpw=x<9l+2hwj1TK z`PEoFDh~*+!%atds^dSM25xex9p3Ihlo|=6vEOk6sH2sC1Ui}kL4Uxu?0{YV0HB#R zgAAa#kRWl37y|O+k`0BAcvBf=bvqHqnNkW0fhLXjCcpV_8PDBo1Vwef^ym7A>UQbh zh0)3N5v*z7ya$-+SFC~;xJal0f@a%;v1Xe#21 zfOhU71B~pniL@Xe3ZIs`N!WB;th!HY`|VyG3o$b4`tVc@9K0bTt^*)>J)(?Qk}%_K7z8Yv?)Q<#vwSgc zfZZc-Ldb>>-222|n7oB5qrb-l?@E|zokQ~kTEtR4>?#?^ZUULc4rT_*f|>1a?~^)P zsC6c+^_1=k_#e42PJk+YehBmUh*Ds%EzRKoXb7E4b8d(OJs~V|rQ|wYAPcEao(_w3 znh?#z7fVA)@dq4xv}2q?qk_9)B+pnL;1KGVlW?MpXy2lU_xhy?S&0vqp2e`K;;PHF zVVXqDG(d5;0DnHTNgLui<-?Y_f#cukQ)|l2x%Ytl3Q@e8*mekhQpX`~1AQ|#6#^Ax zW?h#UNPhE>zoBev7LW7r?xvc=cbS_44o`D2+6e^Jb`|b5`kh?i`zG;ehw`JyUvXZQ z=6hr{kmWKrG(B=}-9IIK-&t1;7dtTfWN83R$%K+H{?=Z%)f6WDZH5$b0h{h>rXH#VRFq$| z!GK*R!ab5eSKWA1@+_#UXkW$E5=+dzk5X^Oqq~l6!h^Dz5#H8{CqUp{s#?cCZ6&zy z5hs%|j0v|5Eq)Q8eQp#W1^sq28HddMp&*XmjhP<6ifZ0uHZg)GPUlc#n`4-DZe98Q zj=VRZ;YdTUw&}?hA0zq6mg;OnYkd&;<+s!{vE`+}o8^mrAfg}=KMIF%?*kX@LzX=a z+4corGPM6_57aMo%*npq4kOkL!nwtXTi!wIux0)}Y<4vugq&#WD|nnii|fD#9j;^U z>juKAEb41Fq(;PpJTJ_I?MHcr%{Jgv0a`%OXcn8`sQC-w08UOsGW+%0_zlloOlN7^ zG`Bq@Y0^J@K8@IvEhZo*^d|&CT+767y&8Nu2<+;&Hg6n!lcQ{vkxUbpwscyU=1!n* zn%d6ER#{q%P|3T$`C?MoNgemw5jUCz3H#wzSKhlt}sbI@*q(7;(y zs##KEYm`q|a>7yC#u=<1Zhxbvz4QO;?9BtI+`cz(=a>#s$CQkjDMdmkauS&`6iH^4 z3>7lZ9WpCYBt(YHnddQ6WsFS8JkMlSB)|3EbnpFr@8|c=uYd3Lp7-5*?Y-A}*7H0& zH~&6$&H!Oxu_B-X*I(eBdO-=V^aeD!-?(7c@gMsjkKw2%smD~@?p^a~!~Pq0N}5Z_NbL2Mkz@voo(ie)yU zKUE*)ZB{GlzO$)98;7a9d~)H)O}d9i*n`<(N76n7F;;x9X0#D>Si$#XTvyw@;>g`o z%UFAK4<{Ap6#vNK`S2q%CN%9lgW?nQ0b)Lb*b(kl!28}-<&V^mTwi|lv@VE-c7f@= zBb{tT%Gj(&wac>II~nyc6J5Z#H1n}hSr5ylmq)gooc(evX$NS`Xol(eW|+35>n)W4 z;&#g+8T{sGH6H`Ti4~y%$SI8`9ToqYx^!;k7ku&r2C?O_R={C>%7>#e@oM0v-J<_~ zhfF<-k=pt+s+pLNHF?xTPJyXH1Vb!~VU}6K_K<}z${ff|V;Undj_Qpd9`zy>@;){8 zM2a;qhnIZkDEqUK%yyj|M@j>3|`Tt{e8Kp=+^sAsFk zem7deu=x3%gI}=s5-Y;g_&miaoUDi*+N_jmNe~?1?H%_=uXyheaj>?)VV$xhQAuPL zw?!q(EIH826pR*>AaaU>9mcU>eieU{g`;3_5@r967v^i}nk>hN?Lz1KZQq&tD8H>C zI-k+zt>Ba$)FF}BrRxudJf)9@J&NN*y;e^@vpT%WiC+9GA>-#fv{9Z=&u-5g%c0VcYE7w(j)eh|r)5cY4hkx-ti%dy{GeFo*p7N-j z2<#@lcnex!r)mg+OXDYrp>>mBdrNa@#HsU8JjoT3kb~ZYn?y2U7AIXyzu6rnCE}Yp zAVjG~T!F=0^mW9NsgIyM#V8VJ4~<}LdU+{~@w?XFrK)HjoRlHfHw<&a`|^3__*C2N zvbv8qW4#U@dVyt7u>uNVu{k~E~_B($T1%kY#GFa(02WK;{j6)Z*3Ls_xwouJFOiE;p}0N zy#)+^^x-(C0K&s1AvwQtE{*DY%hTR%d`>_te2*0O{rKjin`+$T38K|)bjK+@{pv4i z566;MeY;wAG{>>Kb9ZiOMYeot?}IYN&9P*O7Ywr$P!*T!oL0t2bOg`pDhG?I8lZe zZ2{bdxK(>Zz_#s^p=o*^`Z!?QYPuG)v8~)WBt!06K9D~mqi|8PCdixA+3DfPdtB;q zS$dhLp({O7`rA8CBfNAXjPZ$0gR#>xS;v zMh5FV$Rv9c4JCid=*fuGB?i$S!0I&SkCd@ag`OhyVuPJ=BgBpt z=&oHKBMW!ZRf*Tv(D5J3w4ejrZRClwbaBE_-66{sR30JHi_gm;^A=zo8C`rNrgxXD zzDj53Kmlv5WA4@&=Nbx%gYnJULJqOmYam%+$ct5Qa8X7oCqr8PV-RSm= z4Oyrcj0|tHabtH#I;>K=xu-{p#21+cHYXPps!yP`KvMFFqMa1b*~!(l7Go80v0 zT3+9>K7+s$n-mi@O-M&CZba^K8e0J6W^`W?n{l zw;F-DMohaS;(g=u0!h#X*(7F+=}+^2qa9}(w3&~S^}fW14|6-EJt4B{9n*l4Cp^Dy z_k;t}t?_n+>K+8sdik+Q@FHcM37s@qdzA;e(@8?&F46Q;i-PQn2i_Y$!?%*1>Jl*_ z3=L|f7x0?~8?IDe;#yMv|AURkyJvE=dBBb*<4N%D*2S8pnsf3n6h)K&K`;xluT6Xv zBzuZuBeG7G2+pgGoN}Q}w0XFEPV3No4rK=NQEYAh0k@0{7b2B@8Cpyy>WCU_f*=h9 z*mwM*&dTAVTHnWJJW8}Vj*R__@kjgL|97*cx+?n2{b!IqasLQO=sF&hV@PeygOb|Q z0u{w2fCY~I4=bMysqQLw*B?-zB7(+_+*|VDc6f3^nbJLip(Y{xWw`dc zv7p#k1(o*}*;UE)D`7e&-0*puym8aRfwP?{&0swlp7Tn~D)uq6#8t@H`3Ol5YI?0e zsi@>M_IL%cm4m)*_ll*sDf7nR6WVM*^`U1*k%k7?4&Lof=LW=6efSpiC!Rw^-3)D} zBq)&5D?!Jy`qOboP&l~#Hu7%x`5z{!@vd4zF9{a!o-!g=wJ?gq7u@@L70hgS0b;rutNg`z2IMtY|B_au<>P|r z_<10Sy4D3fD2~KSPp+a1Z#OT90BAV&pSX4DkAUb=&NHOO2%xhoH>0RuV%`nrnFJMBbSoi*1cdw*U)cB@Te`>O zDv#J;f)ZZ|P0V1svXgN=dx*3d6c#QIKaxa*OE8uHwd=9}2S>c=>wZirGfx0;&o#^p zi^bW0+wu&xfkjmlaFO_-K-@fzqZiQQdqhGB9+*7}fIfHsQs-imS>-6$g7!%hf;z$f zpw3?P{U>PNeVuR{Q#WJfzS5ALA`49xHTv)P$$9lz}}|Bils*Cf>#^Y$hiK;XC%OO@xp zn?b6ZDvDt@5507~z|#V{BV~{xUzObikEphQoa}HU&n#^2wa&;?7_DS`r|1<=1%iW~`Z9{Yw z_rN4}v9ngf7f&wo*Km%QD8E%n$fvpBTzz*@@tM6hSII3EsB-Oc$gOIxA4{yv}l zOU}rA?5PpgfH>9q@BZpgEGRpI6dOc5AD}6FjT<-J63GcYSENVN1Pir0_S7pD{}U17 z6xJYre8#KB>ip}+bP~c_FS9cbm!pXkkfL+*T_Y^M-=8dSfn|LSdizQMNKIC54Lt`x z-SGMem=k&~#RoDN0janT!}<>ILLT#0vr1Bp^o{gEg#mBYGyb0y1aC$(11-}qG3xi+ zxY4BghYkJ#&-kwkgufv|ufN{=neuacK6`FU+&3PWZdYNq&?M%|0@~F_6VOuH>}=jI zG^CBa&P+`9G#iOyyI;6@hc?dNZ>y1GdK#n+9)+HHdW(xAUXXo8rYDU+W za{%kqefm8WI#^zHi-Qywt^b}FhWfWg*8hKce!|u{hp53T4YuGGv-fb68|MXHf}{MB zwTBj`skEoWf=zip5ZZU4rN;H}*H1KiVEx5qYu2-N4M3Dip&c#3v~Fl&KlEqmpF=u) z2!97;2jP5j{o)@`>?OT>KR(MJP1~D6+4p*hnU%m)9f)#xcBl0=Lbs7PV;*16Dt^qo z!>4iZW%SrdQX!iQDNhm-yRhW4=ziGBzweEMhV`O}kYsBoYw=|uCw;at?!V{Sd z6QH)WV_vxvO|~~LSQ@IK5j(Nf4rRf#i&MG0wku=r^K2%SBwW_b0$J6WaUr+mU<1?g z>vZJ@pdbQK3W3E}m&vm4*ehJYlf;`~{hRD9C>3{5f1Tt&Kg2sqqbgfbSVl$dvhHzG zzm_o92V~UrEE&!^Kyv0***|AW#yPN39`b^5o zjYBLX=DGKbcJ`2}p|0QmR91}lEmP177X8(pA{qTp=&Kff1DG%gl8`3OfE?}HsrO-y z(vKiQB(U*QTq*mUD9_J6Z}kHLT60QoYwsDw)t~+3SLQT#w(k;VWb>MipTJx9BeVs~ zB`f119r+Q9;;!k+17yv>39?}5$tOe>omRC<8}$H@Qw4rybMg;o=S?;-n?I<7ETZ_} zK(e^$J2W9YT9;i_w9&;V5p)EYIZhMBHs7Cr<7S^q9`!u{ysIl##ZObuXWA_`V+6_$ zK7A632gYEW9xH+iA}v65G&X7ZDGzItw(^i2$dN` zUW$mE>9V5L6(erJb6YB(w#n7K2bbp0HPW`3uG$l;nFWND9ia}sJ1yFh8;Vnc$CV1~ zKMcHlOJvP$tkT5P%%RKB&5+uF8Ay<$07GpZ4sLQz+rtMW=IOnHyf6Nnw4+Z=BSiGY zM#F{x1%4=$1gshRQZKv)*&7}uL4yiDgu+Y~dHQw(8@M$+gIymnp|(F1WOw{x;zt!! z`q5F!Yr|nL_5COn#V6+ZLN9P7*oA4{kRJMSoHcvA5A#3lxq1We<$NqatOcRjCCj0o zd$K1n%|6q_Up+SlB20Nn92hC&e9!k(OS~pM|MTh?q4>DQ>rfDd9EeER^Ef$TD^q%A zJrC2L>jfcC*Y%b6j~wG=HQ4eU12d^G5YzDnQ^%J8ti1|y%4TRUs-P^qcKJ@Plhx?2 znehjd)80?p>MYb7Ay)ar1f#3R4&aw_^WMX_o;*WP{>MlA9F=y_y+{#o4i_S$@-_uZ zarZZiFIgvn0|C+^MG`RIl_aD3w;JKhz8c}O!tDB-m`8i#`TYuaIn5f%V zm&?Id+q^qHoLSOoxr)IZnyPvGebg^s5nsy9Z^Pd<&K7jWT_6PV&xHGZ_U3Qz_mxbpr-a&U zmWviCoDXwz12^gvlatOjEsm<_c^Tg~ixTH2CI-8XxKo_Ge?WcLqLz zC|SF=H&LnkPElM;LxV!d^9>$aP~HeYKSaZ;VORz=Cz!iua5R-*Q0FOMY18n zfX);(X!hw(K!@nC;~^v&cmu5QJda(MW9o7EFtC-f94LA4+SIn?rBEI?x_Vm6@(hu( zI?sINxh z|42`(N!f98;YPr3i`Mkq-a*1`=u`8N{7Y4UC>Fq4Ci#=!V2<%4YImdH(h3ll&dsp8 zk*GO>2RUL2ng=HA-N9OiQi}^W|Fm_XWVtqw`)R;uMvz!XX`t{(|B%*fzFb2-Z&m*P zbX+4khO$av1G(|>M@x{}haYhUqQ`9z*x7(00G2Kp4G-T1zsKd`x&=@dDTQ$#s$qdE z2P{mXw?<7z$DM^8Kva1MJ&uTn2=SN?Cy=z6@L$aXs(=B!DF?zq1R_y02-m{Y!Dc9S zjiy8q39)P~0xOSjBJ zqxBlA1xDLj`Qkb>q8qTNz)D-}4k|^>>DRjLnmNg}>iqIVyh% zO#(TeDCR`;A6dQ;_tPi>pBy(JhK(L~H4SXwQX8}04r>@{-#B1uv zulVP_;J>cwEIe9y_iJ7fqSt*gtWmx^b$H)wWm^xq)}a|&1<<_at$!>9gkPo11rZe9 zq@)CP9#D3(WGh_}2V=JNwMf(0P>?Yw{rWzBsHU07Dbf}iOcl+`jPL$Z94LWgWU_eJ z<2iJjy9&~fE;HHWCw{A!Jt;lGbMk`Sn&IpvQ8~HKQ~*rrtH&~v?4JwbJ~#_BEqi3v zQ)SFKr*<|4eRKgZ6p#xQQrop?ojSo*E}NE^Px2l@{wVvzjbs1LMm|WxXwvfXigOB_wqiI!`c1WWmt4@HxJ8DI^lz;`?{G(`q^`E}-i~vm=tB~WT;DdD} zYlnLww4EageMged`~We6#aF)=Mr99G^c`fTgG6@rokKj?*kPu3L97x^Pep8S(c7e! zW{NtlR}ka*1ui78r#$cHq{!+8IuK(qSUS)ci`7{8>ivcGeW6t+_+oJ23|$vRxlghC z>hE0Bg?hfBAh5k}LeYO)kmW@F%TX+s4dV@PHOk>muHjoFP zSwWEC^b|DNX)@OABP(O0%cFH*+pvK0*<$vR?%Kp(#gjBfnER_fLW{^!wwdp1K=4{< zL(z}-;53a+~9gnE`q0HQxgf)b%{n&!qW=mh@_KhWQB z`F#GjC$Lh>*RO;YDU;LpGbMJJ+mDhDhvpO4UGqmMO5s?QU$6P!_N$&5)mC6rhXmJ? z`nLNYd8H6CEtm@nyTW-To0o2$zyhr_&^-u5(n5K<8vS=(Ch@5i_PFPy+{p}ccL!(a z78Q-fDP>b*s>mcz)lQ5;dOfL->f?Rm`ysEcjXkGh&-B8gBHL&8`pRi$jt9Dhj6Vbn z7pIP;W&nyx0%u9R-+6bMFc!_*>JOk+)1<`~Q#q5yo;VxG=g%LOZ#^RT_!4e9_;(ShdwZZH<0Yp0TZAZ@V3lo|V;7Sl@I$$yBbs z$^OU6SxlVAJ-Rt>0yKsVj}IX}P9AqwN84tUMNB}>Fd#wW;zmfYVzj1bGSe-X z+aWuE?YC+Sryt@WJ#)KXI<(F-)%}jI-g1qmvT3(zKw;awpuP-e()P6!4?lF;dDqN` zU#K*Tn#h)&GhRI(ti;4Q5KFNB{uy!FkxQ-7mp*>2%2%x$v>6s@Y;G)kc-rri`bkka zRcC9Xm33YI6v{7RO1_`lhc_Z8tlM79m0$fDOg92NtPnQ5%lPGEFhDR$^kPq zMV5wJqg82>Y26D|XWs{}50@#~mVEATy79`dEm}-jI9m`V>W*>77UbaXn2Sq(9t37Zp6%`IASUQ3oEiyH#y z2nhiKv=DdolPIy)fP7&M>%u~t9;@&WfwIA9U;dP^wc&>{SNjWc^h26dYYKJSp+F6x zNMgLYVJ{!=6KvO20CG`!*DO?)`V&_ z4e6f8#IQINAZL$*;Fss)iTCFk<+kIn=+r}up2zn;Py}V!RbejlD#r1X$L-*I(UNMb zwg)91uAcs+En1i?ad9`3|O?e=C|wO-Ddr%dB~&ETjvYxccM$yJp2 z3t1c_r>tU69D##oEU!G@l{PLL`r=c~bSt%PJt-BAlV_aZ_q<|}csq~Jw++IETMESl z@BDcBQI4L(W_;sB{95eFgthLso%Q4tXG0}bRn=Zo<<8E|5Q=2z7TUe_0)tC>mQc## z6=&PWr2{ln|0`U!(5Xdn0+-j}u@lRiN8hmVehFnuuMrj!m9r30c`h7VRu@X_S2274 z7at6G01Q6X1v?re%qH931w z_OAuX=0S`M&EkX2JLzdvyz9{q1Qo9scustEx^7n3MVof@I(onqrVx? zwN5|zU$N;ru2&Yud2X|*itcq|)x)W8&a7k+&Lw;K;BVzUbheA!ZerdY3&gA=71$Pc9GL*CEt+94 z=Zd~47<;J0zW%QMAUSc8)J3sQNJxdmrX~lqBj|DVqStnPEsrlmj=ZMY+TM z5x2LS!F2jK0|UcpxSL7>Ol4ZPK72SZvvPy*io4SW?yis3gr zEG*K~*1!_*1hDHwD9*{{!AAM{rs4iZiFt(|;+uW)a%g0>HCjNXme0&ON~l8}I~yKe z6{)JM%+HGIj<%+x1lzt->^Ez@$@1KH96l3lL*pVan8&f-OcG%X^ZEKWaQe|1A3pNtq03j zK?a;?SQRwOr|9{BaW87Dxl=;YlGpTQ{`0N3!K*a5#E!~iw0~u1D5451pTsu9@sKO( zf)kH4F3m>IsuOfGdBk+2CSE9;otc6q97vc-5(s{sQQuS|ProM$1oF4bVO*wn(nx`l zorT5wbr_y7@bXgD(t5#A*=49OBl0kYryS_;KY(|B4{g>c=P$F@qo$55hLp`>Qi4-0oDPT)UzbtsQn6aLm*p2h>)22w zJZtlI5k*^YpD4{{SfGCUa*pIF3h(jCgofSH@U*Ojg-BQ;lC4%m#nN(A7v~2Y+ z{q-yZQ{L`6iH#!B@VzB23Wnl0P5Rh5?&4SPY4dks#5&Tv+m0Gf5DUbA?$i5FkixhKy5 zJEz6ezr=S(Hy^o$8JH{?v@{`B*U!{^z1SJbt)H3-)+Kp>ZIKhs+%SzXIb7Po*h4qu8x;4pAha=BL);8UIW-w0Y7E5ax z9Nr*HI3KWp*6TYLB_N=GwM(tJ4PzCB|5r+c$AuA>ls>j0Bzo%<^CC6l(VWF=n5;7A zlRm1UU8erKIiFw0SP@^25u70KaWJp_~>S%!t1F;1-hsDPx==oXFGRV5UE?eI!3(z%?T%}TTQ~4s&B3s z6!#+nz7~G|6I$ipa}fPB;GwPay1hS7j@TQn75lmta^lNOn!3yuhx2m12frop$Q!G*UmdLTV!Hvm<-)R@y2n`CIs>a9>*j)lC z6E{D0ER-~coh#3vQ#Jq^V4dTf?Y@Exx+9pAup zY0=dT+s|LTxYM@^LL6Saj%nt4C{eqTMlR={fxCfc+GHz^6}E@F6Q(EOI2NfvE&xMB zJPJ_G0GKDlbIP*6sFkPDs2&YKFEW9XkmvSXS0Ca&+a7}IVfK3o`eb{-phGO<>eu~5}{HIivi80IE=YV08=5ZpC;Q`Oge zQe|;Lk)SZ~WA6SCVvnFK1qeGeKWcV0rX@3CKuK*WGm2;nUfF&UN{oa)TPTuzBA4#M z79z|UAt51=%nI(^r#KT3pvaki=2*%r4!}aQe+(ta1>FW(4^JFu=lhtSKYeOc6%iR} zUUfZ1mE}4*^|?UJ0>k8&vm%)e(52#WvuHoyqk2Eunn{|oVwUXxBbro;buV4=MGQ=l zjsc_UC+R&mq_~*b1OMeK$gQ5Y4znNsaoG&fae@5&&bo$}yL77Rup8K1zk$2UkW6$e zqouVq9LdYUR&rK}ps;}<-!EJ=^9-%z^XJd`D;!dGbbN?E%pN3a7r*Z$!1Py4dpgm; zih6l+$*z3Ru2Y4_Xl}h9S{TZRv<6VxFa+_voO!YmbEWa&e14+os|UUy?*4fMoPHdm z_J-d*+~?n2fjmW#%eS_5CI&((aX_>_%&SkUQHNKyZ-+RJW-g>iT#EdclshLHjTIHm#yx z{LkM?)o|O`+1{oTGpHLYP{z9Wg_{?K>d{Mca(vFK5=3>CvB+~EcnTY62#88b52)WN z)$WE%{$9$N+7;?j29+7LJ5R@$CXdf~$xJ@MTNHxtkx}fAmgeT?AnghexYw0qd@-LK z>2=7pWw&`ESf^#xk2_j5g`Z!jUe0*(#tAl%1QT~dbgKQi;NSHufhPf(wJk&3hlF%G z^7@+(1*~*31=zZP^hd70uz(Fpc90x6`+{KZ#fj;CgbHN{e=wUdm1<`)_8{Hd#QNrY zxs+~St@O+Vi^A}g+`L25oNrU*JPx2lY=nb(2sK zfSr#(R4X9;LzAXP%Es;jSWmq6S52YZC94e5cs<_I(qb1?o4d9mTr1RMqb5tR@gef+ z@4^np)&v{drBW1H^XJej!`u3sVtK}{MfV0Pl|9Zn;STP(Xp&PNe>J%o) zxm=XUzu{ZZYIbZxW8TTd$78O|1KvzAG4)uq82VZu{Hr(?_~-Eio->&5v%XSi(P28F z)z9qjPPD3BZwz*KePeMmfh>l)Ot~jJN=?RamHLeH+ zyGcjwN+@(%u+&U(1yf*gEWDH6f3{XJPqU$%omLKojg_0}v$}WBheqt>YLb&vaBVeE zHA^0AY<%|eDc_g9sz-KG(~`~8X)T@xC1G#n)l<5)K0I@;poaq0N;Tlm<8ld5UAL_L z0&VjL;mYo3AL&0uNJ&WdlgB0rUVCJw6H|72p@W}`voii7^-wI2R&uvX#wGJ`IyStP zR^aZR(ab?xl0;S~5XFsI7DlTU$E;5M+`F7%-#0egY45i=Gx@bZ?_5N6fzCM>W~MaA z^8a6TW(nPGG$~aMdAU&cdiK$`Pv2#_sU4gZjuo&H&ERc3D)9Yi>0Ipzd1aegAM?WC zGi1C$I6MI(>Tvm=<#LJ`6IXLjEah(IsO_)xEv_y9ol#-Ut*{BM+3rtYu@*_v5u%w| zh9L>rN{Xtg4pGr->{u;4fzro6(fN`-$EJ>Tjqibj=h;Pv?J>F7$8Il@yF=SiF=gk@ zuoR!(jWjQO&{IQbS4z}QAbR-PpS5ro9bRf&>~JU8Mx#0)Em=oe-`cEqopj7N$UlD?K8<9tt~BYHCeurp^D*sEh%h0_6t7$G8mCL6{9q?YGHqlxth{u ztp1*Jc1luTuViYV!rC@(bJ&IAaOVW5=9<9e>m*{%Dq=NMQHze$(q*SQ_gx+Gl)6jt zN`L0`Enefohp##hiUNsVnTff#O}9*b6W>k`51iBQ+1j8xDOT`}c~dGL%Fi%_R6uX& z#tyZ0C^)OKI!3{zqs`zAYXS$s)I7j9a`IvqA4Z+IZ&7H$><71`5vjMO9>e^}Ex1Qy zp9W|Rj+g53CXX11nR`tzH*HYfvS_7^l)5<~w1Hf_X*b&;2Yr4kuW6t;2EIFLsgt=m z4ei%cnLT2eN^%@jZ1_tF@2mFT9;<*to3TZ=IwaYT#xp4IEd_mUYkxn(XSR26Fp!?B zBeW$}%2mm7>77ho@Err&R?$@;az;s;n*&&E)02AT;!ImnISH?S0*q?lciJcW`Q#AB zx^C%7{;-lzo5!!^*PmtIbqR82#$RR?mqjXNsF*Wp>FDULXTr7j9dOv%q;z+@-tBnS zRTl^=B#`mrZ@?7~m%x=6AJVj?yWaDOcA!$?UBgHUkD`G`)i;4Eo$hp-J08~}m#eiK zo14!idlaP97!VMU)hWte&_wRK0(`hxYHL8_PUT|B_k)kf-^aj9E=9efxbz!c{6&OB zvhBIWMVHKEVhrFzfThUH{t~%23~t)Z`P+`Jo z+3+1l|1AG0m^1zO{;DUlH~tYs7O9W~-TwCeU86dKV7o=QtpTuDGp%FYP&NLj0JWS6 z^mT3C(_L=>W*p)sirD%Kx1K(BOc83VyPM_X!RIckgDk#?fy{L#-mhhghK6PmSOnqV zAM>5&OG@FK@Pk(qtP%^IeF1N{zC4w1e3GXroxPo()bs$01|cSHmg7$-uA$*ZofJq} z0ZeG+ORh3VM8FEO&Dl=*b^VYs^BlNAAnwNyG%*pr_|)AQZOT^wt0a7Tcv(e76t(e0 zZ0ISVV-i#Sjg1r)Znb+##uwrh=y==a+Dijvsi|ph*EHn5P@TEQ=sB019?1%a0b{=CvV)ikq0{PG@yeZ6nnVDB$zC&jAQO8 z`$fbmITv+v{P@D-_4lioRLZLd<=E!XMD3|!D7rtNqm6Cv!irCo1Ks*WTfIa$2lSlrAHg1_uQZLYE3NSCSXvZ4G@>fr86mNuPu{% zz=!|qGpG)dKU*A;0xTG@kwg+{pbM1Wa8lUn4!40Cmq@r8*Nb}Dt9K$rc42*y!Uht2F8Ds$Z@;3bvA?t3C8^aWTcB)Q$G z!uxRz>0K#WxhFH!T={V0XrjD3?={Y2iX;vT0&w=cb0Pz<_X?DP{QJWR zZiyS)jx%?hw==B-yaU;cuHXqQsDkVEmp^P>19aAqi`YNsQTPD`@ zC#U$>cl{Xa+-sD5-*rbaMPE0->Gb_3t){%RmD%u(vRK>LlOpAu zbw%XUMnw1EYwn|M#Qz>V{5xXb^J3SE7IzJ}qO1hcIG%m3jx?1fi~F@?)s^LjLi0%x z?Vn9pTy+FmRYkOQs8ZQi<>BeMFXC_{H!4_}m9wgr_GV#tan)Y%N@zyEe}=h4aAZQ^ zwSa<%`wEA|ddAi_;>;UTiX?VMOguH*I(Sg|R>{6K6bdV;M<6)ldEoUULYT7v;!^%)J-?)O2 z<-idC@rxac-XyL=}{Mr$#;`V<{Q?0%x6#{JY_}$ z21FO$~RondUw1y=8Hb}* zv0Oihi91RR2xmjTehQy0%0k=P5GAm|2cXZ&{SzOWCq_&ND7)`ieG&w*2h-OaDcz!_ zA#Q0FNuydbIR3b}xL&s3DAu}f9y+A{>D~+W`@%X(yIp7L3=g=n;j7>;D2La>+N*J~NET6*jTnyBb-4vK=gr z%@^iT2-tVl=1`ED-4>ww6Kt3S0u7bAVn%@r3$0xeTsVRx!GEIa`&E4CdBZal;WE`P zAmR=mCEY)uVPThWL(56|#To}z3RdF!!ZC(FkHp0>qmrY1Ki2q!s62(=y>OcS3MNEUcl`c`b-#>#-@zjPS|APy~$IT5Y+^ur=(zE-iCXOu!eOZ)p z*sg@ANLk|n6+6Cz@}Ez4J65gmS}@v2iQPyUh9xNE(Lb>?K+T3P<8F4Q@{fg-#bq<> z@!v0^9*lhX_(<(Xo(32&wqjQMze5S@e-3rYcRZ`{)|?kvoKW{Gy#s&83hpgbHn%E$ zZtZNt+8C_TWBK!+u-eFXTbiW8{8|mYz)aM)UhzuN{`1`go~y(!Ef|ML>Ca1CI1D>-=|<|gzkiMt%~%4`jaylo x?}AyS`hH3o>`4P=1w21i(vxl69}=qf2)h-kPCnG)UnYQmigK#5Su#dm{|_U5$2R}~ literal 0 HcmV?d00001 diff --git a/doc/index.rst b/doc/index.rst index 196ea93285..91f965d601 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -6,6 +6,8 @@ ros2_control ################# +This is the documentation of the ros2_control framework core. + `GitHub Repository `_ ================= @@ -14,15 +16,12 @@ API Documentation API documentation is parsed by doxygen and can be found `here <../../api/index.html>`_ - - ========= Features ========= * :ref:`Command Line Interface (CLI) ` - ======== Concepts ======== @@ -31,5 +30,6 @@ Concepts :titlesonly: Controller Manager <../controller_manager/doc/userdoc.rst> + Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> From d39ddcdb2a6cf423530f93587646f2ce502b85d4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Jul 2023 21:32:56 +0200 Subject: [PATCH 49/50] Fix equal and higher controller update rate (#1070) --- controller_manager/src/controller_manager.cpp | 17 ++- .../test/test_controller/test_controller.hpp | 2 +- .../test/test_controller_manager.cpp | 121 +++++++++++++++++- 3 files changed, 131 insertions(+), 9 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 58f91a786d..cb875f25de 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -610,6 +610,18 @@ controller_interface::return_type ControllerManager::configure_controller( controller_name, std::make_unique(controller, update_rate_)); } + const auto controller_update_rate = controller->get_update_rate(); + const auto cm_update_rate = get_update_rate(); + if (controller_update_rate > cm_update_rate) + { + RCLCPP_WARN( + get_logger(), + "The controller : %s update rate : %d Hz should be less than or equal to controller " + "manager's update rate : %d Hz!. The controller will be updated at controller_manager's " + "update rate.", + controller_name.c_str(), controller_update_rate, cm_update_rate); + } + // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers if (controller->is_chainable()) { @@ -1873,8 +1885,9 @@ controller_interface::return_type ControllerManager::update( { const auto controller_update_rate = loaded_controller.c->get_update_rate(); - bool controller_go = - controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0); + bool controller_go = controller_update_rate == 0 || + ((update_loop_counter_ % controller_update_rate) == 0) || + (controller_update_rate >= update_rate_); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 1b070ab302..f73f592037 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -65,7 +65,7 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); - size_t internal_counter = 0; + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller // is usually destroyed after cleanup diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 22221b6e21..5a452bb0b1 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -27,13 +27,13 @@ using ::testing::_; using ::testing::Return; -class TestControllerManager +class TestControllerManagerWithStrictness : public ControllerManagerFixture, public testing::WithParamInterface { }; -TEST_P(TestControllerManager, controller_lifecycle) +TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) { const auto test_param = GetParam(); auto test_controller = std::make_shared(); @@ -159,7 +159,7 @@ TEST_P(TestControllerManager, controller_lifecycle) controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); - auto last_internal_counter = test_controller->internal_counter; + size_t last_internal_counter = test_controller->internal_counter; // Stop controller, will take effect at the end of the update function start_controllers = {}; @@ -196,7 +196,7 @@ TEST_P(TestControllerManager, controller_lifecycle) EXPECT_EQ(1, test_controller.use_count()); } -TEST_P(TestControllerManager, per_controller_update_rate) +TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { auto strictness = GetParam().strictness; auto test_controller = std::make_shared(); @@ -254,4 +254,113 @@ TEST_P(TestControllerManager, per_controller_update_rate) } INSTANTIATE_TEST_SUITE_P( - test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort)); + test_strict_best_effort, TestControllerManagerWithStrictness, + testing::Values(strict, best_effort)); + +class TestControllerManagerWithUpdateRates +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_update_rate) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + const unsigned int ctrl_update_rate = GetParam(); + auto test_controller = std::make_shared(); + + auto last_internal_counter = 0u; + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Testing update rate : %u Hz", ctrl_update_rate); + { + ControllerManagerRunner cm_runner(this); + 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( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); + test_controller->get_node()->set_parameter(update_rate_parameter); + // configure controller + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + EXPECT_EQ( + controller_interface::return_type::OK, + 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()); + + // Start controller, will take effect at the end of the update function + 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"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, 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_state().id()); + + const auto pre_internal_counter = test_controller->internal_counter; + rclcpp::Rate loop_rate(cm_->get_update_rate()); + for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) + { + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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())); + EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + + auto deactivate_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, deactivate_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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))) + << "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, deactivate_future.get()); + } + 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()); + last_internal_counter = test_controller->internal_counter; +} + +INSTANTIATE_TEST_SUITE_P( + per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, + testing::Values(100, 232, 400)); From ca33bcc57d3f7138f7263011315c826da30c8a49 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 5 Jul 2023 17:46:13 +0200 Subject: [PATCH 50/50] [CM] Make error message after trying to active controller more informative. (#1066) --- controller_manager/src/controller_manager.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index cb875f25de..c4887db15a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1399,8 +1399,11 @@ void ControllerManager::activate_controllers( if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( - get_logger(), "After activating, controller '%s' is in state '%s', expected Active", - controller->get_node()->get_name(), new_state.label().c_str()); + 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); } if (controller->is_async())