diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index ececdae62c..df11022c50 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -2,7 +2,7 @@ name: Good first issue about: Create an issue to welcome a new contributor into the community. title: '' -labels: good-first-issue +labels: ["good first issue"] assignees: '' --- diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 0e2488d31e..eed2a7589f 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_control-not-released.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index 6392ba8e68..0fabfda92f 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -32,6 +32,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index f3c31703cd..078a541bf0 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index 209c931d4e..8255b02bb4 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -28,6 +28,11 @@ on: - 'ros2_control.humble.repos' - 'codecov.yml' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index 85a1b38241..d613d99ed3 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -17,6 +17,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: debian_source_build: diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index 5bb2408578..38a76ee025 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - humble +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 62c5cd62ba..e986bf361e 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -17,6 +17,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: rhel_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 96938467f9..7b0c5f1fd3 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -1,6 +1,6 @@ name: Humble Semi-Binary Build # author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' +# description: 'Build & test all ros2_control dependencies from source.' on: workflow_dispatch: @@ -32,8 +32,13 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: - binary: + semi-binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: fail-fast: false @@ -45,3 +50,14 @@ jobs: ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: humble + semi-binary-clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_control.humble.repos + ref_for_scheduled_build: humble + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..f0bc3a9fd2 --- /dev/null +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -0,0 +1,49 @@ +name: Humble Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.humble.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_control.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.humble.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_control.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.humble.repos + not_test_downstream: true diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 367b3736fb..aa0fe81e63 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_control-not-released.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index 5be853ebfc..599a075d9a 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -32,6 +32,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index cbdf6c30bd..7910cde0b5 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index 4ec6a29fff..0f695ed8bc 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -17,6 +17,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: debian_source_build: diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml index d9ec610bbc..aab5ba52ac 100644 --- a/.github/workflows/jazzy-pre-commit.yml +++ b/.github/workflows/jazzy-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/jazzy-rhel-binary-build.yml b/.github/workflows/jazzy-rhel-binary-build.yml index 0dcc912dab..a6404d2dbd 100644 --- a/.github/workflows/jazzy-rhel-binary-build.yml +++ b/.github/workflows/jazzy-rhel-binary-build.yml @@ -17,6 +17,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: rhel_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 12769eb740..d5e3a96835 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -1,6 +1,6 @@ name: Jazzy Semi-Binary Build # author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' +# description: 'Build & test all ros2_control dependencies from source.' on: workflow_dispatch: @@ -32,8 +32,13 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: - binary: + semi-binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: fail-fast: false @@ -45,3 +50,14 @@ jobs: ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master + semi-binary-clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_control.jazzy.repos + ref_for_scheduled_build: master + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..3bcf417887 --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -0,0 +1,49 @@ +name: Jazzy Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.jazzy.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.jazzy.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.jazzy.repos + not_test_downstream: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index b7828390fb..50f2a46154 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_control-not-released.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 24a28f16ae..0859169b15 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -32,6 +32,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index bd83c0caca..085ee7f9d1 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -11,6 +11,10 @@ on: - '**.yaml' - '.github/workflows/rolling-check-docs.yml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index b55091521e..35957bbc0e 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -29,6 +29,11 @@ on: - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index 621ffb6a26..bddbd14878 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -29,6 +29,11 @@ on: - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 45b10876e7..ad4e467606 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -28,6 +28,11 @@ on: - 'ros2_control.rolling.repos' - 'codecov.yml' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_rolling: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 00d4ad844b..a54e7ad0d1 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -17,6 +17,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: debian_source_build: diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 7bc07ba802..792278d6d2 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index c8939d6015..95d6759da6 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -17,6 +17,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: rhel_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 492da45ab9..6431b9f4fa 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -1,6 +1,6 @@ name: Rolling Semi-Binary Build # author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' +# description: 'Build & test all ros2_control dependencies from source.' on: workflow_dispatch: @@ -32,8 +32,13 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: - binary: + semi-binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: fail-fast: false @@ -45,3 +50,15 @@ jobs: ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master + semi-binary-clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + # job for building only, no tests -> one distro is enough + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..57db2ae7ba --- /dev/null +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -0,0 +1,57 @@ +name: Rolling Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.rolling.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: true diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml index 07a9e2904a..3452796f55 100644 --- a/.github/workflows/rosdoc2.yml +++ b/.github/workflows/rosdoc2.yml @@ -8,6 +8,9 @@ on: - ros2_control/rosdoc2.yaml - ros2_control/package.xml +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true jobs: check: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 205e0f63ab..75c5402ffe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 665b0175f6..32795abf9d 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Fixed typo. Added s to state_interfaces\_ (`#1930 `_) +* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 `_) +* Contributors: Christoph Fröhlich, louietouie + +4.21.0 (2024-12-06) +------------------- +* [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (`#1871 `_) +* Contributors: Sai Kishor Kothakota + 4.20.0 (2024-11-08) ------------------- * reset the async variables upon activation to work post exceptions (`#1860 `_) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index ae3804919c..b47027bb31 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -71,6 +71,25 @@ struct ControllerUpdateStats unsigned int total_triggers; unsigned int failed_triggers; }; + +/** + * Struct to store the status of the controller update method. + * The status contains information if the update was triggered successfully, the result of the + * update method and the execution duration of the update method. The status is used to provide + * feedback to the controller_manager. + * @var successful: true if the update was triggered successfully, false if not. + * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. + * @var execution_time: duration of the execution of the update method. + * @var period: period of the update method. + */ +struct ControllerUpdateStatus +{ + bool successful = true; + return_type result = return_type::OK; + std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; +}; + /** * Base interface class for an controller. The interface may not be used to implement a controller. * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface` @@ -110,7 +129,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * The configuration is used to check if controller can be activated and to claim interfaces from * hardware. * The claimed interfaces are populated in the - * \ref ControllerInterfaceBase::state_interface_ "state_interface_" member. + * \ref ControllerInterfaceBase::state_interfaces_ "state_interfaces_" member. * * \returns configuration of state interfaces. */ @@ -175,13 +194,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * * \param[in] time The time at the start of this control loop iteration * \param[in] period The measured time taken by the last control loop iteration - * \returns A pair with the first element being a boolean indicating if the async callback method - * was triggered and the second element being the last return value of the async function. For - * more details check the AsyncFunctionHandler implementation in `realtime_tools` package. + * \returns ControllerUpdateStatus. The status contains information if the update was triggered + * successfully, the result of the update method and the execution duration of the update method. */ CONTROLLER_INTERFACE_PUBLIC - std::pair trigger_update( - const rclcpp::Time & time, const rclcpp::Duration & period); + ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); diff --git a/controller_interface/include/semantic_components/force_torque_sensor.hpp b/controller_interface/include/semantic_components/force_torque_sensor.hpp index ab55a537ad..18c3948962 100644 --- a/controller_interface/include/semantic_components/force_torque_sensor.hpp +++ b/controller_interface/include/semantic_components/force_torque_sensor.hpp @@ -25,26 +25,23 @@ namespace semantic_components { +constexpr std::size_t FORCES_SIZE = 3; class ForceTorqueSensor : public SemanticComponentInterface { public: /// Constructor for "standard" 6D FTS - explicit ForceTorqueSensor(const std::string & name) : SemanticComponentInterface(name, 6) + explicit ForceTorqueSensor(const std::string & name) + : SemanticComponentInterface( + name, + // If 6D FTS use standard names + {{name + "/" + "force.x"}, + {name + "/" + "force.y"}, + {name + "/" + "force.z"}, + {name + "/" + "torque.x"}, + {name + "/" + "torque.y"}, + {name + "/" + "torque.z"}}), + existing_axes_({{true, true, true, true, true, true}}) { - // If 6D FTS use standard names - interface_names_.emplace_back(name_ + "/" + "force.x"); - interface_names_.emplace_back(name_ + "/" + "force.y"); - interface_names_.emplace_back(name_ + "/" + "force.z"); - interface_names_.emplace_back(name_ + "/" + "torque.x"); - interface_names_.emplace_back(name_ + "/" + "torque.y"); - interface_names_.emplace_back(name_ + "/" + "torque.z"); - - // Set all interfaces existing - std::fill(existing_axes_.begin(), existing_axes_.end(), true); - - // Set default force and torque values to NaN - std::fill(forces_.begin(), forces_.end(), std::numeric_limits::quiet_NaN()); - std::fill(torques_.begin(), torques_.end(), std::numeric_limits::quiet_NaN()); } /// Constructor for 6D FTS with custom interface names. @@ -62,7 +59,8 @@ class ForceTorqueSensor : public SemanticComponentInterface::quiet_NaN()); - std::fill(torques_.begin(), torques_.end(), std::numeric_limits::quiet_NaN()); } - virtual ~ForceTorqueSensor() = default; - /// Return forces. /** * Return forces of a FTS. * - * \return array of size 3 with force values. + * \return array of size 3 with force values (x, y, z). */ - std::array get_forces() + std::array get_forces() const { - size_t interface_counter = 0; - for (size_t i = 0; i < 3; ++i) + std::array forces; + forces.fill(std::numeric_limits::quiet_NaN()); + std::size_t interface_counter{0}; + for (auto i = 0u; i < forces.size(); ++i) { if (existing_axes_[i]) { - forces_[i] = state_interfaces_[interface_counter].get().get_value(); + forces[i] = state_interfaces_[interface_counter].get().get_value(); ++interface_counter; } } - return forces_; + return forces; } /// Return torque. /** * Return torques of a FTS. * - * \return array of size 3 with torque values. + * \return array of size 3 with torque values (x, y, z). */ - std::array get_torques() + std::array get_torques() const { + std::array torques; + torques.fill(std::numeric_limits::quiet_NaN()); + // find out how many force interfaces are being used // torque interfaces will be found from the next index onward - auto torque_interface_counter = - std::count(existing_axes_.begin(), existing_axes_.begin() + 3, true); + auto torque_interface_counter = static_cast( + std::count(existing_axes_.begin(), existing_axes_.begin() + FORCES_SIZE, true)); - for (size_t i = 3; i < 6; ++i) + for (auto i = 0u; i < torques.size(); ++i) { - if (existing_axes_[i]) + if (existing_axes_[i + FORCES_SIZE]) { - torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value(); + torques[i] = state_interfaces_[torque_interface_counter].get().get_value(); ++torque_interface_counter; } } - return torques_; + return torques; } /// Return Wrench message with forces and torques. @@ -141,29 +138,24 @@ class ForceTorqueSensor : public SemanticComponentInterface existing_axes_; - std::array forces_; - std::array torques_; }; } // namespace semantic_components diff --git a/controller_interface/include/semantic_components/imu_sensor.hpp b/controller_interface/include/semantic_components/imu_sensor.hpp index 74c5524826..793e5c0fea 100644 --- a/controller_interface/include/semantic_components/imu_sensor.hpp +++ b/controller_interface/include/semantic_components/imu_sensor.hpp @@ -27,73 +27,68 @@ namespace semantic_components class IMUSensor : public SemanticComponentInterface { public: - explicit IMUSensor(const std::string & name) : SemanticComponentInterface(name, 10) + explicit IMUSensor(const std::string & name) + : SemanticComponentInterface( + name, {{name + "/" + "orientation.x"}, + {name + "/" + "orientation.y"}, + {name + "/" + "orientation.z"}, + {name + "/" + "orientation.w"}, + {name + "/" + "angular_velocity.x"}, + {name + "/" + "angular_velocity.y"}, + {name + "/" + "angular_velocity.z"}, + {name + "/" + "linear_acceleration.x"}, + {name + "/" + "linear_acceleration.y"}, + {name + "/" + "linear_acceleration.z"}}) { - interface_names_.emplace_back(name_ + "/" + "orientation.x"); - interface_names_.emplace_back(name_ + "/" + "orientation.y"); - interface_names_.emplace_back(name_ + "/" + "orientation.z"); - interface_names_.emplace_back(name_ + "/" + "orientation.w"); - interface_names_.emplace_back(name_ + "/" + "angular_velocity.x"); - interface_names_.emplace_back(name_ + "/" + "angular_velocity.y"); - interface_names_.emplace_back(name_ + "/" + "angular_velocity.z"); - interface_names_.emplace_back(name_ + "/" + "linear_acceleration.x"); - interface_names_.emplace_back(name_ + "/" + "linear_acceleration.y"); - interface_names_.emplace_back(name_ + "/" + "linear_acceleration.z"); - - // Set default values to NaN - orientation_.fill(std::numeric_limits::quiet_NaN()); - angular_velocity_.fill(std::numeric_limits::quiet_NaN()); - linear_acceleration_.fill(std::numeric_limits::quiet_NaN()); } - - virtual ~IMUSensor() = default; - /// Return orientation. /** * Return orientation reported by an IMU * - * \return array of size 4 with orientation quaternion (x,y,z,w) + * \return Array of size 4 with orientation quaternion (x,y,z,w). */ - std::array get_orientation() + std::array get_orientation() const { - size_t interface_offset = 0; - for (size_t i = 0; i < orientation_.size(); ++i) + std::array orientation; + for (auto i = 0u; i < orientation.size(); ++i) { - orientation_[i] = state_interfaces_[interface_offset + i].get().get_value(); + orientation[i] = state_interfaces_[i].get().get_value(); } - return orientation_; + return orientation; } /// Return angular velocity. /** * Return angular velocity reported by an IMU * - * \return array of size 3 with angular velocity values. + * \return array of size 3 with angular velocity values (x, y, z). */ - std::array get_angular_velocity() + std::array get_angular_velocity() const { - size_t interface_offset = orientation_.size(); - for (size_t i = 0; i < angular_velocity_.size(); ++i) + std::array angular_velocity; + const std::size_t interface_offset{4}; + for (auto i = 0u; i < angular_velocity.size(); ++i) { - angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value(); + angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_value(); } - return angular_velocity_; + return angular_velocity; } /// Return linear acceleration. /** * Return linear acceleration reported by an IMU * - * \return array of size 3 with linear acceleration values. + * \return array of size 3 with linear acceleration values (x, y, z). */ - std::array get_linear_acceleration() + std::array get_linear_acceleration() const { - size_t interface_offset = orientation_.size() + angular_velocity_.size(); - for (size_t i = 0; i < linear_acceleration_.size(); ++i) + std::array linear_acceleration; + const std::size_t interface_offset{7}; + for (auto i = 0u; i < linear_acceleration.size(); ++i) { - linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value(); + linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_value(); } - return linear_acceleration_; + return linear_acceleration; } /// Return Imu message with orientation, angular velocity and linear acceleration @@ -101,36 +96,29 @@ class IMUSensor : public SemanticComponentInterface * Constructs and return a IMU message from the current values. * \return imu message from values; */ - bool get_values_as_message(sensor_msgs::msg::Imu & message) + bool get_values_as_message(sensor_msgs::msg::Imu & message) const { - // call get_orientation() and get_angular_velocity() get_linear_acceleration() to - // update with the latest values - get_orientation(); - get_angular_velocity(); - get_linear_acceleration(); + const auto [orientation_x, orientation_y, orientation_z, orientation_w] = get_orientation(); + const auto [angular_velocity_x, angular_velocity_y, angular_velocity_z] = + get_angular_velocity(); + const auto [linear_acceleration_x, linear_acceleration_y, linear_acceleration_z] = + get_linear_acceleration(); - // update the message values, covariances unknown - message.orientation.x = orientation_[0]; - message.orientation.y = orientation_[1]; - message.orientation.z = orientation_[2]; - message.orientation.w = orientation_[3]; + message.orientation.x = orientation_x; + message.orientation.y = orientation_y; + message.orientation.z = orientation_z; + message.orientation.w = orientation_w; - message.angular_velocity.x = angular_velocity_[0]; - message.angular_velocity.y = angular_velocity_[1]; - message.angular_velocity.z = angular_velocity_[2]; + message.angular_velocity.x = angular_velocity_x; + message.angular_velocity.y = angular_velocity_y; + message.angular_velocity.z = angular_velocity_z; - message.linear_acceleration.x = linear_acceleration_[0]; - message.linear_acceleration.y = linear_acceleration_[1]; - message.linear_acceleration.z = linear_acceleration_[2]; + message.linear_acceleration.x = linear_acceleration_x; + message.linear_acceleration.y = linear_acceleration_y; + message.linear_acceleration.z = linear_acceleration_z; return true; } - -protected: - // Order is: orientation X,Y,Z,W angular velocity X,Y,Z and linear acceleration X,Y,Z - std::array orientation_; - std::array angular_velocity_; - std::array linear_acceleration_; }; } // namespace semantic_components diff --git a/controller_interface/include/semantic_components/pose_sensor.hpp b/controller_interface/include/semantic_components/pose_sensor.hpp index 60dbecd718..098fb04278 100644 --- a/controller_interface/include/semantic_components/pose_sensor.hpp +++ b/controller_interface/include/semantic_components/pose_sensor.hpp @@ -28,38 +28,31 @@ class PoseSensor : public SemanticComponentInterface { public: /// Constructor for a standard pose sensor with interface names set based on sensor name. - explicit PoseSensor(const std::string & name) : SemanticComponentInterface{name, 7} + explicit PoseSensor(const std::string & name) + : SemanticComponentInterface( + name, {{name + '/' + "position.x"}, + {name + '/' + "position.y"}, + {name + '/' + "position.z"}, + {name + '/' + "orientation.x"}, + {name + '/' + "orientation.y"}, + {name + '/' + "orientation.z"}, + {name + '/' + "orientation.w"}}) { - // Use standard interface names - interface_names_.emplace_back(name_ + '/' + "position.x"); - interface_names_.emplace_back(name_ + '/' + "position.y"); - interface_names_.emplace_back(name_ + '/' + "position.z"); - interface_names_.emplace_back(name_ + '/' + "orientation.x"); - interface_names_.emplace_back(name_ + '/' + "orientation.y"); - interface_names_.emplace_back(name_ + '/' + "orientation.z"); - interface_names_.emplace_back(name_ + '/' + "orientation.w"); - - // Set all sensor values to default value NaN - std::fill(position_.begin(), position_.end(), std::numeric_limits::quiet_NaN()); - std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits::quiet_NaN()); } - - virtual ~PoseSensor() = default; - /// Update and return position. /*! * Update and return current pose position from state interfaces. * * \return Array of position coordinates. */ - std::array get_position() + std::array get_position() const { - for (size_t i = 0; i < 3; ++i) + std::array position; + for (auto i = 0u; i < position.size(); ++i) { - position_[i] = state_interfaces_[i].get().get_value(); + position[i] = state_interfaces_[i].get().get_value(); } - - return position_; + return position; } /// Update and return orientation @@ -68,41 +61,36 @@ class PoseSensor : public SemanticComponentInterface * * \return Array of orientation coordinates in xyzw convention. */ - std::array get_orientation() + std::array get_orientation() const { - for (size_t i = 3; i < 7; ++i) + std::array orientation; + const std::size_t interface_offset{3}; + for (auto i = 0u; i < orientation.size(); ++i) { - orientation_[i - 3] = state_interfaces_[i].get().get_value(); + orientation[i] = state_interfaces_[interface_offset + i].get().get_value(); } - - return orientation_; + return orientation; } /// Fill pose message with current values. /** * Fill a pose message with current position and orientation from the state interfaces. */ - bool get_values_as_message(geometry_msgs::msg::Pose & message) + bool get_values_as_message(geometry_msgs::msg::Pose & message) const { - // Update state from state interfaces - get_position(); - get_orientation(); + const auto [position_x, position_y, position_z] = get_position(); + const auto [orientation_x, orientation_y, orientation_z, orientation_w] = get_orientation(); - // Set message values from current state - message.position.x = position_[0]; - message.position.y = position_[1]; - message.position.z = position_[2]; - message.orientation.x = orientation_[0]; - message.orientation.y = orientation_[1]; - message.orientation.z = orientation_[2]; - message.orientation.w = orientation_[3]; + message.position.x = position_x; + message.position.y = position_y; + message.position.z = position_z; + message.orientation.x = orientation_x; + message.orientation.y = orientation_y; + message.orientation.z = orientation_z; + message.orientation.w = orientation_w; return true; } - -protected: - std::array position_; - std::array orientation_; }; } // namespace semantic_components diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index e4af8b7f86..df4b0f46c5 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -26,30 +26,25 @@ namespace semantic_components class RangeSensor : public SemanticComponentInterface { public: - explicit RangeSensor(const std::string & name) : SemanticComponentInterface(name, 1) + explicit RangeSensor(const std::string & name) + : SemanticComponentInterface(name, {name + "/" + "range"}) { - interface_names_.emplace_back(name_ + "/" + "range"); } - - virtual ~RangeSensor() = default; - /** * Return Range reported by a sensor * * \return value of the range in meters */ - float get_range() { return state_interfaces_[0].get().get_value(); } + float get_range() const { return state_interfaces_[0].get().get_value(); } /// Return Range message with range in meters /** * Constructs and return a Range message from the current values. * \return Range message from values; */ - bool get_values_as_message(sensor_msgs::msg::Range & message) + bool get_values_as_message(sensor_msgs::msg::Range & message) const { - // update the message values message.range = get_range(); - return true; } }; diff --git a/controller_interface/include/semantic_components/semantic_component_interface.hpp b/controller_interface/include/semantic_components/semantic_component_interface.hpp index e763aa5d93..1f49e8cbff 100644 --- a/controller_interface/include/semantic_components/semantic_component_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_interface.hpp @@ -27,14 +27,20 @@ template class SemanticComponentInterface { public: - explicit SemanticComponentInterface(const std::string & name, size_t size = 0) + SemanticComponentInterface( + const std::string & name, const std::vector & interface_names) + : name_(name), interface_names_(interface_names) + { + state_interfaces_.reserve(interface_names.size()); + } + + explicit SemanticComponentInterface(const std::string & name, std::size_t size = 0) : name_(name) { - name_ = name; interface_names_.reserve(size); state_interfaces_.reserve(size); } - ~SemanticComponentInterface() = default; + virtual ~SemanticComponentInterface() = default; /// Assign loaned state interfaces from the hardware. /** @@ -85,7 +91,7 @@ class SemanticComponentInterface return false; } // insert all the values - for (size_t i = 0; i < state_interfaces_.size(); ++i) + for (auto i = 0u; i < state_interfaces_.size(); ++i) { values.emplace_back(state_interfaces_[i].get().get_value()); } diff --git a/controller_interface/package.xml b/controller_interface/package.xml index ca3fc2af49..d3a40a227c 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.20.0 + 4.22.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 0713ec3c25..03d45fa384 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -34,7 +34,9 @@ return_type ControllerInterfaceBase::init( try { - auto_declare("update_rate", update_rate_); + // no rclcpp::ParameterValue unsigned int specialization + auto_declare("update_rate", static_cast(update_rate_)); + auto_declare("is_async", false); auto_declare("thread_priority", 50); } @@ -123,8 +125,8 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } if (is_async_) { - const unsigned int thread_priority = - static_cast(get_node()->get_parameter("thread_priority").as_int()); + const int thread_priority = + static_cast(get_node()->get_parameter("thread_priority").as_int()); RCLCPP_INFO( get_node()->get_logger(), "Starting async handler with scheduler priority: %d", thread_priority); @@ -159,12 +161,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } -std::pair ControllerInterfaceBase::trigger_update( +ControllerUpdateStatus ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { + ControllerUpdateStatus status; trigger_stats_.total_triggers++; if (is_async()) { + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); if (!result.first) { @@ -174,12 +178,28 @@ std::pair ControllerInterfaceBase::trigger_update( "The controller missed %u update cycles out of %u total triggers.", trigger_stats_.failed_triggers, trigger_stats_.total_triggers); } - return result; + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } } else { - return std::make_pair(true, update(time, period)); + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = update(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } + return status; } std::shared_ptr ControllerInterfaceBase::get_node() diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 376b15014d..64b1567dd2 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Async Hardware Components (`#1567 `_) +* Add controller node options args to be able to set controller specific node arguments (`#1713 `_) +* Use singleton approach to store and reuse the service clients (`#1949 `_) +* Increase the max and min periodicity tolerances to fix flaky tests (`#1937 `_) +* Fix the spawner to support full wildcard parameter entries (`#1933 `_) +* Suppress unnecessary warnings in clock received validation (`#1935 `_) +* Optimize the valid time check in the update loop (`#1923 `_) +* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 `_) +* Update CPU affinity parameter to be able to set multiple CPUs (`#1915 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Takashi Sato + +4.21.0 (2024-12-06) +------------------- +* Use the .hpp headers from realtime_tools package (`#1916 `_) +* CM: Check if a valid time is received (`#1901 `_) +* Lock memory by default on a realtime system setup (`#1896 `_) +* Fix the launch_utils regression (`#1909 `_) +* [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (`#1871 `_) +* Add more parameter overriding tests by parsing multiple parameter files (`#1899 `_) +* add logic for 'params_file' to handle both string and string_array (`#1898 `_) +* [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) +* Add documentation on `ros2_control_node` and make lock_memory false by default (`#1890 `_) +* Add service call timeout argument in spawner (`#1808 `_) +* Add CM `switch_controller` service timeout as parameter to spawner.py (`#1790 `_) +* Fix the missing bcolors.ENDC in hardware_spawner log prints (`#1870 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Tony Najjar + 4.20.0 (2024-11-08) ------------------- * change from thread_priority.hpp to realtime_helpers.hpp (`#1829 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 9544a7ceab..dfcc437eb6 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -16,6 +16,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp realtime_tools std_msgs + libstatistics_collector + generate_parameter_library ) find_package(ament_cmake REQUIRED) @@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(controller_manager_parameters + src/controller_manager_parameters.yaml +) + add_library(controller_manager SHARED src/controller_manager.cpp ) @@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC $ $ ) +target_link_libraries(controller_manager PUBLIC + controller_manager_parameters +) ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, @@ -49,10 +58,12 @@ target_link_libraries(ros2_control_node PRIVATE if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(example_interfaces REQUIRED) # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_link_libraries(test_controller PUBLIC controller_manager) + ament_target_dependencies(test_controller PUBLIC example_interfaces) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) install( @@ -202,14 +213,17 @@ if(BUILD_TESTING) ) target_link_libraries(test_hardware_spawner controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) - install(FILES test/test_controller_spawner_with_fallback_controllers.yaml - DESTINATION test) - - install(FILES test/test_controller_spawner_with_type.yaml - DESTINATION test) + install(FILES + test/test_controller_spawner_with_type.yaml + test/test_controller_overriding_parameters.yaml + test/test_controller_spawner_with_fallback_controllers.yaml + test/test_controller_spawner_wildcard_entries.yaml + test/test_controller_spawner_with_interfaces.yaml + DESTINATION test) ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp @@ -234,7 +248,7 @@ install( DESTINATION include/controller_manager ) install( - TARGETS controller_manager + TARGETS controller_manager controller_manager_parameters EXPORT export_controller_manager RUNTIME DESTINATION bin LIBRARY DESTINATION lib diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 4a8d7daee5..638a28ce86 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,9 +23,9 @@ set_hardware_component_state, switch_controllers, unload_controller, - get_parameter_from_param_file, + get_parameter_from_param_files, set_controller_parameters, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) @@ -40,8 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", - "get_parameter_from_param_file", + "get_parameter_from_param_files", "set_controller_parameters", - "set_controller_parameters_from_param_file", + "set_controller_parameters_from_param_files", "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 909e681ce6..36a924f101 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -55,6 +55,35 @@ class ServiceNotFoundError(Exception): pass +class SingletonServiceCaller: + """ + Singleton class to call services of controller manager. + + This class is used to create a service client for a given service name. + If the service client already exists, it returns the existing client. + It is used to avoid creating multiple service clients for the same service name. + + It needs Node object, service type and fully qualified service name to create a service client. + + """ + + _clients = {} + + def __new__(cls, node, service_type, fully_qualified_service_name): + if (node, fully_qualified_service_name) not in cls._clients: + cls._clients[(node, fully_qualified_service_name)] = node.create_client( + service_type, fully_qualified_service_name + ) + node.get_logger().debug( + f"{bcolors.MAGENTA}Creating a new service client : {fully_qualified_service_name} with node : {node.get_name()}{bcolors.ENDC}" + ) + + node.get_logger().debug( + f"{bcolors.OKBLUE}Returning the existing service client : {fully_qualified_service_name} for node : {node.get_name()}{bcolors.ENDC}" + ) + return cls._clients[(node, fully_qualified_service_name)] + + def service_caller( node, service_name, @@ -92,7 +121,7 @@ def service_caller( fully_qualified_service_name = ( f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name ) - cli = node.create_client(service_type, fully_qualified_service_name) + cli = SingletonServiceCaller(node, service_type, fully_qualified_service_name) while not cli.service_is_ready(): node.get_logger().info( @@ -123,7 +152,9 @@ def service_caller( ) -def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def configure_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -132,10 +163,11 @@ def configure_controller(node, controller_manager_name, controller_name, service ConfigureController, request, service_timeout, + call_timeout, ) -def list_controllers(node, controller_manager_name, service_timeout=0.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): request = ListControllers.Request() return service_caller( node, @@ -143,10 +175,11 @@ def list_controllers(node, controller_manager_name, service_timeout=0.0): ListControllers, request, service_timeout, + call_timeout, ) -def list_controller_types(node, controller_manager_name, service_timeout=0.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): request = ListControllerTypes.Request() return service_caller( node, @@ -154,10 +187,13 @@ def list_controller_types(node, controller_manager_name, service_timeout=0.0): ListControllerTypes, request, service_timeout, + call_timeout, ) -def list_hardware_components(node, controller_manager_name, service_timeout=0.0): +def list_hardware_components( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): request = ListHardwareComponents.Request() return service_caller( node, @@ -165,10 +201,13 @@ def list_hardware_components(node, controller_manager_name, service_timeout=0.0) ListHardwareComponents, request, service_timeout, + call_timeout, ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): +def list_hardware_interfaces( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -176,10 +215,13 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0) ListHardwareInterfaces, request, service_timeout, + call_timeout, ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def load_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = LoadController.Request() request.name = controller_name return service_caller( @@ -188,10 +230,13 @@ def load_controller(node, controller_manager_name, controller_name, service_time LoadController, request, service_timeout, + call_timeout, ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): +def reload_controller_libraries( + node, controller_manager_name, force_kill, service_timeout=0.0, call_timeout=10.0 +): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -200,11 +245,17 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ReloadControllerLibraries, request, service_timeout, + call_timeout, ) def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 + node, + controller_manager_name, + component_name, + lifecyle_state, + service_timeout=0.0, + call_timeout=10.0, ): request = SetHardwareComponentState.Request() request.name = component_name @@ -215,6 +266,7 @@ def set_hardware_component_state( SetHardwareComponentState, request, service_timeout, + call_timeout, ) @@ -226,6 +278,7 @@ def switch_controllers( strict, activate_asap, timeout, + call_timeout=10.0, ): request = SwitchController.Request() request.activate_controllers = activate_controllers @@ -237,11 +290,17 @@ def switch_controllers( request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() return service_caller( - node, f"{controller_manager_name}/switch_controller", SwitchController, request + node, + f"{controller_manager_name}/switch_controller", + SwitchController, + request, + call_timeout=call_timeout, ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def unload_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = UnloadController.Request() request.name = controller_name return service_caller( @@ -250,60 +309,101 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti UnloadController, request, service_timeout, + call_timeout, ) -def get_parameter_from_param_file( - node, controller_name, namespace, parameter_file, parameter_name +def get_params_files_with_controller_parameters( + node, controller_name: str, namespace: str, parameter_files: list ): - with open(parameter_file) as f: - namespaced_controller = ( - f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" - ) - WILDCARD_KEY = "/**" - ROS_PARAMS_KEY = "ros__parameters" - parameters = yaml.safe_load(f) - controller_param_dict = None - # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' - for key in [ - controller_name, - namespaced_controller, - f"{WILDCARD_KEY}/{controller_name}", - f"{WILDCARD_KEY}{namespaced_controller}", - ]: - if key in parameters: - if key == controller_name and namespace != "/": - node.get_logger().fatal( - f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" - ) + controller_parameter_files = [] + for parameter_file in parameter_files: + if parameter_file in controller_parameter_files: + continue + with open(parameter_file) as f: + namespaced_controller = ( + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" + ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" + parameters = yaml.safe_load(f) + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if parameter_file in controller_parameter_files: break - controller_param_dict = parameters[key] - - if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: - controller_param_dict = parameters[WILDCARD_KEY][key] - - if controller_param_dict and ( - not isinstance(controller_param_dict, dict) - or ROS_PARAMS_KEY not in controller_param_dict - ): - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" - ) - if ( - controller_param_dict - and ROS_PARAMS_KEY in controller_param_dict - and parameter_name in controller_param_dict[ROS_PARAMS_KEY] - ): - break - - if controller_param_dict is None: - node.get_logger().fatal( - f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_parameter_files.append(parameter_file) + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_parameter_files.append(parameter_file) + if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]: + controller_parameter_files.append(parameter_file) + return controller_parameter_files + + +def get_parameter_from_param_files( + node, controller_name: str, namespace: str, parameter_files: list, parameter_name: str +): + for parameter_file in parameter_files: + with open(parameter_file) as f: + namespaced_controller = ( + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) - if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: - return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" + parameters = yaml.safe_load(f) + controller_param_dict = None + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_param_dict = parameters[key] + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY][key] + if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY] + + if controller_param_dict and ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" + ) + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): + break - return None + if controller_param_dict and parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}" + ) + return None def set_controller_parameters( @@ -347,26 +447,36 @@ def set_controller_parameters( return True -def set_controller_parameters_from_param_file( - node, controller_manager_name, controller_name, parameter_file, namespace=None +def set_controller_parameters_from_param_files( + node, controller_manager_name: str, controller_name: str, parameter_files: list, namespace=None ): - if parameter_file: - spawner_namespace = namespace if namespace else node.get_namespace() + spawner_namespace = namespace if namespace else node.get_namespace() + controller_parameter_files = get_params_files_with_controller_parameters( + node, controller_name, spawner_namespace, parameter_files + ) + if controller_parameter_files: set_controller_parameters( - node, controller_manager_name, controller_name, "params_file", parameter_file + node, + controller_manager_name, + controller_name, + "params_file", + controller_parameter_files, ) - controller_type = get_parameter_from_param_file( - node, controller_name, spawner_namespace, parameter_file, "type" + controller_type = get_parameter_from_param_files( + node, controller_name, spawner_namespace, controller_parameter_files, "type" ) - if controller_type: - if not set_controller_parameters( - node, controller_manager_name, controller_name, "type", controller_type - ): - return False + if controller_type and not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False - fallback_controllers = get_parameter_from_param_file( - node, controller_name, spawner_namespace, parameter_file, "fallback_controllers" + fallback_controllers = get_parameter_from_param_files( + node, + controller_name, + spawner_namespace, + controller_parameter_files, + "fallback_controllers", ) if fallback_controllers: if not set_controller_parameters( diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index c64b893156..9c8f5a9d24 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -20,7 +20,7 @@ def generate_controllers_spawner_launch_description( - controller_names: list, controller_params_file=None, extra_spawner_args=[] + controller_names: list, controller_params_files=None, extra_spawner_args=[] ): """ Generate launch description for loading a controller using spawner. @@ -37,8 +37,8 @@ def generate_controllers_spawner_launch_description( # Passing controller parameter file to load the controller (Controller type is retrieved from config file) generate_controllers_spawner_launch_description( ['joint_state_broadcaster'], - controller_params_file=os.path.join(get_package_share_directory('my_pkg'), - 'config', 'controller_params.yaml'), + controller_params_files=[os.path.join(get_package_share_directory('my_pkg'), + 'config', 'controller_params.yaml')], extra_spawner_args=[--load-only] ) @@ -62,8 +62,10 @@ def generate_controllers_spawner_launch_description( ] ) - if controller_params_file: - spawner_arguments += ["--param-file", controller_params_file] + if controller_params_files: + for controller_params_file in controller_params_files: + if controller_params_file: + spawner_arguments += ["--param-file", controller_params_file] # Setting --unload-on-kill if launch arg unload_on_kill is "true" # See https://github.com/ros2/launch/issues/290 @@ -98,11 +100,51 @@ def generate_controllers_spawner_launch_description( ) +def generate_controllers_spawner_launch_description_from_dict( + controller_info_dict: dict, extra_spawner_args=[] +): + """ + Generate launch description for loading a controller using spawner. + + controller_info_dict: dict + A dictionary with the following info: + - controller_name: str + The name of the controller to load as the key + - controller_params_file: str or list or None + The path to the controller parameter file or a list of paths to multiple parameter files + or None if no parameter file is needed as the value of the key + If a list is passed, the controller parameters will be overloaded in same order + extra_spawner_args: list + A list of extra arguments to pass to the controller spawner + """ + if not type(controller_info_dict) is dict: + raise ValueError(f"Invalid controller_info_dict type parsed {controller_info_dict}") + controller_names = controller_info_dict.keys() + controller_params_files = [] + for controller_name in controller_names: + controller_params_file = controller_info_dict[controller_name] + if controller_params_file: + if type(controller_params_file) is list: + controller_params_files.extend(controller_params_file) + elif type(controller_params_file) is str: + controller_params_files.append(controller_params_file) + else: + raise ValueError( + f"Invalid controller_params_file type parsed in the dict {controller_params_file}" + ) + return generate_controllers_spawner_launch_description( + controller_names=controller_names, + controller_params_files=controller_params_files, + extra_spawner_args=extra_spawner_args, + ) + + def generate_load_controller_launch_description( controller_name: str, controller_params_file=None, extra_spawner_args=[] ): + controller_params_files = [controller_params_file] if controller_params_file else None return generate_controllers_spawner_launch_description( controller_names=[controller_name], - controller_params_file=controller_params_file, + controller_params_files=controller_params_files, extra_spawner_args=extra_spawner_args, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 7e9fe3443b..58eb7be198 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,7 +26,8 @@ load_controller, switch_controllers, unload_controller, - set_controller_parameters_from_param_file, + set_controller_parameters, + set_controller_parameters_from_param_files, bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -60,8 +61,12 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): - controllers = list_controllers(node, controller_manager, service_timeout).controller +def is_controller_loaded( + node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0 +): + controllers = list_controllers( + node, controller_manager, service_timeout, call_timeout + ).controller return any(c.name == controller_name for c in controllers) @@ -79,8 +84,11 @@ def main(args=None): parser.add_argument( "-p", "--param-file", - help="Controller param file to be loaded into controller node before configure", + help="Controller param file to be loaded into controller node before configure. " + "Pass multiple times to load different files for different controllers or to " + "override the parameters of the same controller.", default=None, + action="append", required=False, ) parser.add_argument( @@ -110,7 +118,7 @@ def main(args=None): ) parser.add_argument( "--controller-manager-timeout", - help="Time to wait for the controller manager", + help="Time to wait for the controller manager service to be available", required=False, default=0.0, type=float, @@ -124,6 +132,13 @@ def main(args=None): default=5.0, type=float, ) + parser.add_argument( + "--service-call-timeout", + help="Time to wait for the service response from the controller manager", + required=False, + default=10.0, + type=float, + ) parser.add_argument( "--activate-as-group", help="Activates all the parsed controllers list together instead of one by one." @@ -131,17 +146,26 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--controller-ros-args", + help="The --ros-args to be passed to the controller node for remapping topics etc", + default=None, + required=False, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager - param_file = args.param_file + param_files = args.param_file controller_manager_timeout = args.controller_manager_timeout + service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout - if param_file and not os.path.isfile(param_file): - raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) + if param_files: + for param_file in param_files: + if not os.path.isfile(param_file): + raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) node = Node("spawner_" + controller_names[0]) @@ -174,7 +198,11 @@ def main(args=None): for controller_name in controller_names: if is_controller_loaded( - node, controller_manager_name, controller_name, controller_manager_timeout + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, ): node.get_logger().warn( bcolors.WARNING @@ -182,12 +210,21 @@ def main(args=None): + bcolors.ENDC ) else: - if param_file: - if not set_controller_parameters_from_param_file( + if controller_ros_args := args.controller_ros_args: + if not set_controller_parameters( node, controller_manager_name, controller_name, - param_file, + "node_options_args", + controller_ros_args.split(), + ): + return 1 + if param_files: + if not set_controller_parameters_from_param_files( + node, + controller_manager_name, + controller_name, + param_files, spawner_namespace, ): return 1 @@ -207,7 +244,13 @@ def main(args=None): ) if not args.load_only: - ret = configure_controller(node, controller_manager_name, controller_name) + ret = configure_controller( + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, + ) if not ret.ok: node.get_logger().error( bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC @@ -223,10 +266,11 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( - bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC + f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}" ) return 1 @@ -247,10 +291,11 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( - bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC + f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}" ) return 1 @@ -279,6 +324,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -312,6 +358,7 @@ def main(args=None): node.get_logger().fatal(str(err)) return 1 finally: + node.destroy_node() rclpy.shutdown() diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml new file mode 100644 index 0000000000..a015765c79 --- /dev/null +++ b/controller_manager/doc/parameters_context.yaml @@ -0,0 +1,21 @@ +hardware_components_initial_state: | + Map of parameters for controlled lifecycle management of hardware components. + The names of the components are defined as attribute of ````-tag in ``robot_description``. + Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. + Detailed explanation of each parameter is given below. + The full structure of the map is given in the following example: + + .. code-block:: yaml + + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +diagnostics.threshold.controllers.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.controllers.execution_time: | + The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle. diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index ca222d68c0..f6967930eb 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String] Parameters ----------- -hardware_components_initial_state - Map of parameters for controlled lifecycle management of hardware components. - The names of the components are defined as attribute of ````-tag in ``robot_description``. - Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. - Detailed explanation of each parameter is given below. - The full structure of the map is given in the following example: - -.. code-block:: yaml - - hardware_components_initial_state: - unconfigured: - - "arm1" - - "arm2" - inactive: - - "base3" - -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. - -update_rate (mandatory; integer) - The frequency of controller manager's real-time update loop. - This loop reads states from hardware, updates controller and writes commands to hardware. - .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. @@ -99,6 +73,16 @@ update_rate (mandatory; integer) The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. It is recommended to test the fallback strategy in simulation before deploying it on the real robot. +.. generate_parameter_library_details:: + ../src/controller_manager_parameters.yaml + parameters_context.yaml + +**An example parameter file:** + +.. generate_parameter_library_default:: + ../src/controller_manager_parameters.yaml + + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -158,7 +142,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--controller-ros-args CONTROLLER_ROS_ARGS] controller_names [controller_names ...] positional arguments: @@ -169,73 +153,97 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node -p PARAM_FILE, --param-file PARAM_FILE - Controller param file to be loaded into controller node before configure + Controller param file to be loaded into controller node before configure. Pass multiple times to load different files for different controllers or to override the parameters of the same controller. -n NAMESPACE, --namespace NAMESPACE DEPRECATED Namespace for the controller_manager and the controller(s) --load-only Only load the controller and leave unconfigured. --inactive Load and configure the controller, however do not activate them -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT - Time to wait for the controller manager + Time to wait for the controller manager service to be available + --service-call-timeout SERVICE_CALL_TIMEOUT + Time to wait for the service response from the controller manager --switch-timeout SWITCH_TIMEOUT Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --controller-ros-args CONTROLLER_ROS_ARGS + The --ros-args to be passed to the controller node for remapping topics etc The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: + .. code-block:: yaml + + /**: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + + command_interfaces: + - position + ..... + + position_trajectory_controller_joint1: + ros__parameters: + joints: + - joint1 + + position_trajectory_controller_joint2: + ros__parameters: + joints: + - joint2 + .. code-block:: yaml /**/position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... .. code-block:: yaml /position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... .. code-block:: yaml position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... .. code-block:: yaml /rrbot_1/position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... ``unspawner`` ^^^^^^^^^^^^^^^^ @@ -323,6 +331,39 @@ The workaround for this is to specify another node name remap rule in the ``Node auto cm = std::make_shared( executor, "_target_node_name", "some_optional_namespace", options); +Launching controller_manager with ros2_control_node +--------------------------------------------------- + +The controller_manager can be launched with the ros2_control_node executable. The following example shows how to launch the controller_manager with the ros2_control_node executable: + +.. code-block:: python + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + +The ros2_control_node executable uses the following parameters from the ``controller_manager`` node: + +lock_memory (optional; bool; default: false for a non-realtime kernel, true for a realtime kernel) + Locks the memory of the ``controller_manager`` node at startup to physical RAM in order to avoid page faults + and to prevent the node from being swapped out to disk. + Find more information about the setup for memory locking in the following link : `How to set ulimit values `_ + The following command can be used to set the memory locking limit temporarily : ``ulimit -l unlimited``. + +cpu_affinity (optional; int (or) int_array;) + Sets the CPU affinity of the ``controller_manager`` node to the specified CPU core. + If it is an integer, the node's affinity will be set to the specified CPU core. + If it is an array of integers, the node's affinity will be set to the specified set of CPU cores. + +thread_priority (optional; int; default: 50) + Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99. + +use_sim_time (optional; bool; default: false) + Enables the use of simulation time in the ``controller_manager`` node. + Concepts ----------- diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 068eefc1f9..d33b167997 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -50,6 +50,8 @@ namespace controller_manager { +class ParamListener; +class Params; using ControllersListIterator = std::vector::const_iterator; CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); @@ -220,15 +222,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; - /// Deletes all async controllers and components. - /** - * Needed to join the threads immediately after the control loop is ended - * to avoid unnecessary iterations. Otherwise - * the threads will be joined only when the controller manager gets destroyed. - */ - CONTROLLER_MANAGER_PUBLIC - void shutdown_async_controllers_and_components(); - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -346,7 +339,7 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; - unsigned int update_rate_ = 100; + unsigned int update_rate_; std::vector> chained_controllers_configuration_; std::unique_ptr resource_manager_; @@ -357,6 +350,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); void init_controller_manager(); + void initialize_parameters(); + /** * Clear request lists used when switching controllers. The lists are shared between "callback" * and "control loop" threads. @@ -473,6 +468,8 @@ class ControllerManager : public rclcpp::Node */ rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + std::shared_ptr cm_param_listener_; + std::shared_ptr params_; diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; @@ -603,6 +600,8 @@ class ControllerManager : public rclcpp::Node rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + controller_manager::MovingAverageStatistics periodicity_stats_; + struct SwitchParams { void reset() diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 0f44867814..9cc508772d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -24,9 +24,13 @@ #include #include "controller_interface/controller_interface_base.hpp" #include "hardware_interface/controller_info.hpp" +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" namespace controller_manager { + +using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -35,9 +39,18 @@ namespace controller_manager */ struct ControllerSpec { + ControllerSpec() + { + last_update_cycle_time = std::make_shared(0, 0, RCL_CLOCK_UNINITIALIZED); + execution_time_statistics = std::make_shared(); + periodicity_statistics = std::make_shared(); + } + hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; + std::shared_ptr execution_time_statistics; + std::shared_ptr periodicity_statistics; }; struct ControllerChainSpec diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 18189d5d16..b23ca17d4b 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.20.0 + 4.22.0 Description of controller_manager Bence Magyar Denis Štogl @@ -28,12 +28,15 @@ ros2param ros2run std_msgs + libstatistics_collector + generate_parameter_library ament_cmake_gmock ament_cmake_pytest python3-coverage hardware_interface_testing ros2_control_test_assets + example_interfaces ament_cmake diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 71ca554116..0455015240 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -28,6 +28,8 @@ #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" +#include "controller_manager_parameters.hpp" + namespace // utility { static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; @@ -275,6 +277,7 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } @@ -283,10 +286,6 @@ ControllerManager::ControllerManager( bool activate_all_hw_components, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), - update_rate_(get_parameter_or("update_rate", 100)), - resource_manager_(std::make_unique( - urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), - activate_all_hw_components, update_rate_)), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -296,6 +295,10 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); + resource_manager_ = std::make_unique( + urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), + activate_all_hw_components, params_->update_rate); init_controller_manager(); } @@ -314,18 +317,13 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } void ControllerManager::init_controller_manager() { // Get parameters needed for RT "update" loop to work - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - if (is_resource_manager_initialized()) { init_services(); @@ -351,6 +349,7 @@ void ControllerManager::init_controller_manager() robot_description_subscription_->get_topic_name()); // Setup diagnostics + periodicity_stats_.Reset(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -362,6 +361,25 @@ void ControllerManager::init_controller_manager() &ControllerManager::controller_manager_diagnostic_callback); } +void ControllerManager::initialize_parameters() +{ + // Initialize parameters + try + { + cm_param_listener_ = std::make_shared( + this->get_node_parameters_interface(), this->get_logger()); + params_ = std::make_shared(cm_param_listener_->get_params()); + update_rate_ = static_cast(params_->update_rate); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + this->get_logger(), + "Exception thrown while initializing controller manager parameters: %s \n", e.what()); + throw e; + } +} + void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { RCLCPP_INFO(get_logger(), "Received robot description from topic."); @@ -403,51 +421,52 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; auto set_components_to_state = - [&](const std::string & parameter_name, rclcpp_lifecycle::State state) + [&](const std::vector & components_to_set, 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) { - for (const auto & component : components_to_set) + if (component.empty()) { - 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 + continue; + } + if (components_to_activate.find(component) == components_to_activate.end()) + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + if ( + resource_manager_->set_component_state(component, state) == + hardware_interface::return_type::ERROR) { - RCLCPP_INFO( - get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), - state.label().c_str()); - if ( - resource_manager_->set_component_state(component, state) == - hardware_interface::return_type::ERROR) - { - throw std::runtime_error( - "Failed to set the initial state of the component : " + component + " to " + - state.label()); - } - components_to_activate.erase(component); + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + state.label()); } + components_to_activate.erase(component); } } }; + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + // unconfigured (loaded only) set_components_to_state( - "hardware_components_initial_state.unconfigured", + params_->hardware_components_initial_state.unconfigured, rclcpp_lifecycle::State( State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) set_components_to_state( - "hardware_components_initial_state.inactive", + params_->hardware_components_initial_state.inactive, rclcpp_lifecycle::State( State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); @@ -583,22 +602,36 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.type = controller_type; controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + controller_spec.execution_time_statistics = std::make_shared(); + controller_spec.periodicity_statistics = std::make_shared(); // We have to fetch the parameters_file at the time of loading the controller, because this way we // can load them at the creation of the LifeCycleNode and this helps in using the features such as // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::string parameters_file; + controller_spec.info.parameters_files.clear(); - // Check if parameter has been declared - if (!has_parameter(param_name)) + // get_parameter checks if parameter has been declared/set + rclcpp::Parameter params_files_parameter; + if (get_parameter(param_name, params_files_parameter)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING); - } - if (get_parameter(param_name, parameters_file) && !parameters_file.empty()) - { - controller_spec.info.parameters_file = parameters_file; + if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { + controller_spec.info.parameters_files = params_files_parameter.as_string_array(); + } + else if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { + controller_spec.info.parameters_files.push_back(params_files_parameter.as_string()); + } + else + { + RCLCPP_ERROR( + get_logger(), + "The 'params_file' param needs to be a string or a string array for '%s', but it is of " + "type %s", + controller_name.c_str(), params_files_parameter.get_type_name().c_str()); + } } const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; @@ -621,6 +654,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.fallback_controllers_names = fallback_controllers; } + const std::string node_options_args_param = controller_name + ".node_options_args"; + std::vector node_options_args; + if (!has_parameter(node_options_args_param)) + { + declare_parameter(node_options_args_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(node_options_args_param, node_options_args) && !node_options_args.empty()) + { + controller_spec.info.node_options_args = node_options_args; + } + return add_controller_impl(controller_spec); } @@ -1455,6 +1499,7 @@ controller_interface::return_type ControllerManager::switch_controller( to = controllers; // update the claimed interface controller info + auto switch_result = controller_interface::return_type::OK; for (auto & controller : to) { if (is_controller_active(controller.c)) @@ -1475,6 +1520,32 @@ controller_interface::return_type ControllerManager::switch_controller( { controller.info.claimed_interfaces.clear(); } + if ( + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) != + activate_request_.end()) + { + if (!is_controller_active(controller.c)) + { + RCLCPP_ERROR( + get_logger(), "Could not activate controller : '%s'", controller.info.name.c_str()); + switch_result = controller_interface::return_type::ERROR; + } + } + /// @note The following is the case of the real controllers that are deactivated and doesn't + /// include the chained controllers that are deactivated and activated + if ( + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) != + deactivate_request_.end() && + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) == + activate_request_.end()) + { + if (is_controller_active(controller.c)) + { + RCLCPP_ERROR( + get_logger(), "Could not deactivate controller : '%s'", controller.info.name.c_str()); + switch_result = controller_interface::return_type::ERROR; + } + } } // switch lists @@ -1484,8 +1555,10 @@ controller_interface::return_type ControllerManager::switch_controller( clear_requests(); - RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); - return controller_interface::return_type::OK; + RCLCPP_DEBUG_EXPRESSION( + get_logger(), switch_result == controller_interface::return_type::OK, + "Successfully switched controllers"); + return switch_result; } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl( @@ -1717,6 +1790,7 @@ void ControllerManager::activate_controllers( get_logger(), "Resource conflict for controller '%s'. Command interface '%s' is already claimed.", controller_name.c_str(), command_interface.c_str()); + command_loans.clear(); assignment_successful = false; break; } @@ -1731,6 +1805,7 @@ void ControllerManager::activate_controllers( "Caught exception of type : %s while claiming the command interfaces. Can't activate " "controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + command_loans.clear(); assignment_successful = false; break; } @@ -1782,6 +1857,8 @@ void ControllerManager::activate_controllers( try { + found_it->periodicity_statistics->Reset(); + found_it->execution_time_statistics->Reset(); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -1798,6 +1875,7 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + controller->release_interfaces(); continue; } catch (...) @@ -1805,6 +1883,7 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught unknown exception while activating the controller '%s'", controller_name.c_str()); + controller->release_interfaces(); continue; } @@ -2263,6 +2342,7 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + periodicity_stats_.AddMeasurement(1.0 / period.seconds()); auto [ok, failed_hardware_names] = resource_manager_->read(time, period); if (!ok) @@ -2350,6 +2430,25 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; + // Check for valid time + if (!get_clock()->started()) + { + if (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + throw std::runtime_error( + "No clock received, and time argument is zero. Check your controller_manager node's " + "clock configuration (use_sim_time parameter) and if a valid clock source is " + "available. Also pass a proper time argument to the update method."); + } + + // this can happen with use_sim_time=true until the /clock is received + rclcpp::Clock clock = rclcpp::Clock(); + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 1000, + "No clock received, using time argument instead! Check your node's clock " + "configuration (use_sim_time parameter) and if a valid clock source is available"); + } + std::vector failed_controllers_list; for (const auto & loaded_controller : rt_controller_list) { @@ -2374,13 +2473,15 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - const rclcpp::Time current_time = get_clock()->now(); + bool first_update_cycle = false; + const rclcpp::Time current_time = get_clock()->started() ? get_clock()->now() : time; if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { - // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; + // last_update_cycle_time is zero after activation + first_update_cycle = true; + *loaded_controller.last_update_cycle_time = current_time; RCLCPP_DEBUG( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); @@ -2398,7 +2499,7 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (controller_actual_period.seconds() * controller_update_rate >= 0.99); + (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2412,8 +2513,20 @@ controller_interface::return_type ControllerManager::update( // Catch exceptions thrown by the controller update function try { - std::tie(trigger_status, controller_ret) = - loaded_controller.c->trigger_update(time, controller_actual_period); + const auto trigger_result = + loaded_controller.c->trigger_update(current_time, controller_actual_period); + trigger_status = trigger_result.successful; + controller_ret = trigger_result.result; + if (trigger_status && trigger_result.execution_time.has_value()) + { + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (!first_update_cycle && trigger_status && trigger_result.period.has_value()) + { + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / trigger_result.period.value().seconds()); + } } catch (const std::exception & e) { @@ -2652,11 +2765,6 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } -void ControllerManager::shutdown_async_controllers_and_components() -{ - resource_manager_->shutdown_async_components(); -} - void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { @@ -3090,34 +3198,140 @@ void ControllerManager::controller_activity_diagnostic_callback( std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); bool all_active = true; + const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + const std::string state_suffix = ".state"; + + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + auto make_stats_string = + [](const auto & statistics_data, const std::string & measurement_unit) -> std::string + { + std::ostringstream oss; + oss << std::fixed << std::setprecision(2); + oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " + << statistics_data.max << "] " << measurement_unit + << ", StdDev: " << statistics_data.standard_deviation; + return oss.str(); + }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_controllers; + std::vector bad_periodicity_async_controllers; for (size_t i = 0; i < controllers.size(); ++i) { + const bool is_async = controllers[i].c->is_async(); if (!is_controller_active(controllers[i].c)) { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); + stat.add( + controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); + if (is_controller_active(controllers[i].c)) + { + const auto periodicity_stats = controllers[i].periodicity_statistics->GetStatistics(); + const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics(); + stat.add( + controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(controllers[i].c->get_update_rate()) + " Hz"); + const double periodicity_error = std::abs( + periodicity_stats.average - static_cast(controllers[i].c->get_update_rate())); + if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.error || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + } + const double max_exp_exec_time = is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.error || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_controllers.push_back(controllers[i].info.name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_controllers.push_back(controllers[i].info.name); + } + } + } + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + + if (!high_exec_time_controllers.empty()) + { + std::string high_exec_time_controllers_string; + for (const auto & controller : high_exec_time_controllers) + { + high_exec_time_controllers_string.append(controller); + high_exec_time_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); + } + if (!bad_periodicity_async_controllers.empty()) + { + std::string bad_periodicity_async_controllers_string; + for (const auto & controller : bad_periodicity_async_controllers) + { + bad_periodicity_async_controllers_string.append(controller); + bad_periodicity_async_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with bad periodicity : [ " + bad_periodicity_async_controllers_string + "]"); } if (!atleast_one_hw_active) { - stat.summary( + stat.mergeSummary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are currently active to activate controllers"); } - else + else if (controllers.empty()) { - if (controllers.empty()) - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); - } - else - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::OK, - all_active ? "All controllers are active" : "Not all controllers are active"); - } + stat.mergeSummary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); } } @@ -3170,7 +3384,14 @@ void ControllerManager::hardware_components_diagnostic_callback( void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + const std::string periodicity_stat_name = "periodicity"; + const auto cm_stats = periodicity_stats_.GetStatistics(); stat.add("update_rate", std::to_string(get_update_rate())); + stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); + stat.add( + periodicity_stat_name + ".standard_deviation", std::to_string(cm_stats.standard_deviation)); + stat.add(periodicity_stat_name + ".min", std::to_string(cm_stats.min)); + stat.add(periodicity_stat_name + ".max", std::to_string(cm_stats.max)); if (is_resource_manager_initialized()) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); @@ -3189,6 +3410,27 @@ void ControllerManager::controller_manager_diagnostic_callback( "Resource Manager is not initialized properly!"); } } + + const double periodicity_error = std::abs(cm_stats.average - get_update_rate()); + const std::string diag_summary = + "Controller Manager has bad periodicity : " + std::to_string(cm_stats.average) + + " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz"; + if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.error) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.warn || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); + } } void ControllerManager::update_list_with_controller_chain( @@ -3290,14 +3532,26 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } - if (controller.info.parameters_file.has_value()) + // Add deprecation notice if the arguments are from the controller_manager node + if ( + check_for_element(node_options_arguments, RCL_REMAP_FLAG) || + check_for_element(node_options_arguments, RCL_SHORT_REMAP_FLAG)) + { + RCLCPP_WARN( + get_logger(), + "The use of remapping arguments to the controller_manager node is deprecated. Please use the " + "'--controller-ros-args' argument of the spawner to pass remapping arguments to the " + "controller node."); + } + + for (const auto & parameters_file : controller.info.parameters_files) { if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); - node_options_arguments.push_back(controller.info.parameters_file.value()); + node_options_arguments.push_back(parameters_file); } // ensure controller's `use_sim_time` parameter matches controller_manager's @@ -3312,6 +3566,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back("use_sim_time:=true"); } + // Add options parsed through the spawner + if ( + !controller.info.node_options_args.empty() && + !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG)) + { + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + } + for (const auto & arg : controller.info.node_options_args) + { + node_options_arguments.push_back(arg); + } + std::string arguments; arguments.reserve(1000); for (const auto & arg : node_options_arguments) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml new file mode 100644 index 0000000000..1bb9b152b1 --- /dev/null +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -0,0 +1,136 @@ +controller_manager: + update_rate: { + type: int, + default_value: 100, + read_only: true, + description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." + } + + hardware_components_initial_state: + unconfigured: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be only loaded when controller manager is started. These hardware components will need to be configured and activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + inactive: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be configured when controller manager is started. These hardware components will need to be activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + diagnostics: + threshold: + controller_manager: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + controllers: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index f8347df952..75a98d19dd 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -59,17 +59,33 @@ int main(int argc, char ** argv) const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); - const bool lock_memory = cm->get_parameter_or("lock_memory", true); - std::string message; - if (lock_memory && !realtime_tools::lock_memory(message)) + const bool has_realtime = realtime_tools::has_realtime_kernel(); + const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); + if (lock_memory) { - RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + const auto lock_result = realtime_tools::lock_memory(); + if (!lock_result.first) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory: '%s'", lock_result.second.c_str()); + } } - const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1); - if (cpu_affinity >= 0) + rclcpp::Parameter cpu_affinity_param; + if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) { - const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity); + std::vector cpus = {}; + if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + cpus = {static_cast(cpu_affinity_param.as_int())}; + } + else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) + { + const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); + std::for_each( + cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), + [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); + } + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); if (!affinity_result.first) { RCLCPP_WARN( @@ -109,7 +125,7 @@ int main(int argc, char ** argv) next_iteration_time{cm_now}; // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now(); + rclcpp::Time previous_time = cm->now() - period; while (rclcpp::ok()) { @@ -134,8 +150,6 @@ int main(int argc, char ** argv) std::this_thread::sleep_until(next_iteration_time); } } - - cm->shutdown_async_controllers_and_components(); }); executor->add_node(cm); diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index f4f59ad9df..018f78b457 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -22,7 +22,7 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "controller_manager/visibility_control.h" #include "rclcpp/subscription.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "semantic_components/imu_sensor.hpp" #include "std_msgs/msg/float64_multi_array.hpp" diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 04ae8c02c2..ee8377f2a3 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -87,7 +87,7 @@ controller_interface::return_type TestController::update( command_interfaces_[i].get_name().c_str()); return controller_interface::return_type::ERROR; } - RCLCPP_INFO( + RCLCPP_DEBUG( get_node()->get_logger(), "Setting value of command interface '%s' to %f", command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); command_interfaces_[i].set_value(external_commands_for_testing_[i]); @@ -101,6 +101,48 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + auto ctrl_node = get_node(); + if (!ctrl_node->has_parameter("command_interfaces")) + { + ctrl_node->declare_parameter("command_interfaces", std::vector({})); + } + if (!ctrl_node->has_parameter("state_interfaces")) + { + ctrl_node->declare_parameter("state_interfaces", std::vector({})); + } + const std::vector command_interfaces = + ctrl_node->get_parameter("command_interfaces").as_string_array(); + const std::vector state_interfaces = + ctrl_node->get_parameter("state_interfaces").as_string_array(); + if (!command_interfaces.empty() || !state_interfaces.empty()) + { + cmd_iface_cfg_.names.clear(); + state_iface_cfg_.names.clear(); + for (const auto & cmd_itf : command_interfaces) + { + cmd_iface_cfg_.names.push_back(cmd_itf); + } + cmd_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + external_commands_for_testing_.resize(command_interfaces.size(), 0.0); + for (const auto & state_itf : state_interfaces) + { + state_iface_cfg_.names.push_back(state_itf); + } + state_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + } + + const std::string service_name = get_node()->get_name() + std::string("/set_bool"); + service_ = get_node()->create_service( + service_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); + response->success = request->data; + }); + return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d57fd9ddd9..ee9e668cfa 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,11 +15,14 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +#include #include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" +#include "example_interfaces/srv/set_bool.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace test_controller @@ -68,10 +71,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC std::vector get_state_interface_data() const; - const std::string & getRobotDescription() const; - void set_external_commands_for_testing(const std::vector & commands); + rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 0c4e51985f..49c02d28ac 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -366,6 +366,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -373,6 +374,21 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); EXPECT_EQ(test_controller->internal_counter, 1u); + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) + << "The first trigger cycle should have zero period"; + + const double exp_period = (cm_->get_clock()->now() - time_).seconds(); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 1u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 2u); + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period)))); size_t last_internal_counter = test_controller->internal_counter; // Stop controller, will take effect at the end of the update function @@ -576,7 +592,26 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // [cm_update_rate, 2*cm_update_rate) EXPECT_THAT( test_controller->update_period_.seconds(), - testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); + testing::AllOf(testing::Ge(0.85 / cm_update_rate), testing::Lt((1.15 / cm_update_rate)))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf( + testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf( + testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf( + testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((2.0 * cm_->get_update_rate())))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -683,6 +718,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto initial_counter = test_controller->internal_counter; // don't start with zero to check if the period is correct if controller is activated anytime rclcpp::Time time = time_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { rclcpp::Time old_time = time; @@ -691,19 +728,36 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); - // In case of a non perfect divisor, the update period should respect the rule - // [controller_update_rate, 2*controller_update_rate) - EXPECT_THAT( - test_controller->update_period_.seconds(), - testing::AllOf( - testing::Gt(0.99 * controller_period), - testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) - << "update_counter: " << update_counter << " desired controller period: " << controller_period - << " actual controller period: " << test_controller->update_period_.seconds(); - if (update_counter % cm_update_rate == 0) + if (test_controller->internal_counter - initial_counter > 0) + { + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter + << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); + } + else + { + // Check that the first cycle update period is zero + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) { const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it // is clearly tracking, so adding 1 here won't affect the final count. @@ -711,10 +765,26 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. EXPECT_THAT( - test_controller->internal_counter - initial_counter, - testing::AnyOf( - testing::Ge((controller_update_rate - 1) * no_of_secs_passed), - testing::Lt((controller_update_rate * no_of_secs_passed)))); + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 50.0); // 50 microseconds } } } @@ -723,6 +793,148 @@ INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90)); +class TestAsyncControllerUpdateRates +: public ControllerManagerFixture +{ +}; + +TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_and_stats) +{ + const unsigned int ctrl_update_rate = cm_->get_update_rate() / 2; + auto test_controller = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + test_controller->get_node()->set_parameter({"is_async", true}); + // configure controller + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } + ASSERT_TRUE(test_controller->is_async()); + time_ = test_controller->get_node()->now(); // set to something nonzero + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + const auto cm_update_rate = cm_->get_update_rate(); + const auto controller_update_rate = test_controller->get_update_rate(); + const double controller_period = 1.0 / controller_update_rate; + + const auto initial_counter = test_controller->internal_counter; + // don't start with zero to check if the period is correct if controller is activated anytime + rclcpp::Time time = time_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); + for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) + { + rclcpp::Time old_time = time; + cm_->get_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + + // the async controllers will have to wait for one cycle to have the correct update period in + // the controller + if (test_controller->internal_counter - initial_counter > 1) + { + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter + << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); + } + // else + // { + // // Check that the first cycle update period is zero + // EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + // } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) + { + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); + // NOTE: here EXPECT_THAT is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_THAT( + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 12000); // more or less 12 milliseconds considering the waittime in the controller + } + } +} + class TestControllerManagerFallbackControllers : public ControllerManagerFixture, public testing::WithParamInterface diff --git a/controller_manager/test/test_controller_overriding_parameters.yaml b/controller_manager/test/test_controller_overriding_parameters.yaml new file mode 100644 index 0000000000..115d0ed993 --- /dev/null +++ b/controller_manager/test/test_controller_overriding_parameters.yaml @@ -0,0 +1,5 @@ +ctrl_with_parameters_and_type: + ros__parameters: + interface_name: "impedance" + joint_offset: 0.2 + joint_names: ["joint10"] diff --git a/controller_manager/test/test_controller_spawner_wildcard_entries.yaml b/controller_manager/test/test_controller_spawner_wildcard_entries.yaml new file mode 100644 index 0000000000..1a05ac03d6 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_wildcard_entries.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + param1: 1.0 + param2: 2.0 + +wildcard_ctrl_3: + ros__parameters: + param3: 3.0 diff --git a/controller_manager/test/test_controller_spawner_with_interfaces.yaml b/controller_manager/test/test_controller_spawner_with_interfaces.yaml new file mode 100644 index 0000000000..05b650c232 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_interfaces.yaml @@ -0,0 +1,25 @@ +ctrl_with_joint2_command_interface: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint2/velocity" + +ctrl_with_joint1_command_interface: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint1/position" + +ctrl_with_joint1_and_joint2_command_interfaces: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint1/position" + - "joint2/velocity" + +ctrl_with_state_interfaces: + ros__parameters: + type: "controller_manager/test_controller" + state_interfaces: + - "joint1/position" + - "joint2/position" diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 087994bd23..23fd69b216 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -2,6 +2,7 @@ ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint0"] + interface_name: "position" /**: chainable_ctrl_with_parameters_and_type: diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c7b67e0cfe..bdd48f15ae 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -65,6 +65,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; + SetUpSrvsCMExecutor(); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -72,11 +73,10 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - - SetUpSrvsCMExecutor(); } void check_component_fileds( diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 62f9f5acb9..552850b5a8 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -85,7 +85,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); @@ -189,7 +189,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 74e1efeeed..ff39b69e8c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,50 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } +TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + cm_->set_parameter(rclcpp::Parameter( + "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter( + rclcpp::Parameter("ctrl_with_parameters_and_type.params_file", test_file_path)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner("ctrl_with_parameters_and_type --load-only -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position"); +} + TEST_F(TestLoadController, spawner_test_type_in_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + @@ -278,7 +322,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0], + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -290,7 +335,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); EXPECT_EQ( @@ -308,7 +353,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0], + test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); @@ -316,10 +362,181 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); } +TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_wildcard_entries.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_1 wildcard_ctrl_2 wildcard_ctrl_3 -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0); + + auto verify_ctrl_parameter = [](const auto & ctrl_node, bool has_param_3) + { + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint1"})); + + if (!ctrl_node->has_parameter("param1")) + { + ctrl_node->declare_parameter("param1", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param1").as_double(), 1.0); + + if (!ctrl_node->has_parameter("param2")) + { + ctrl_node->declare_parameter("param2", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param2").as_double(), 2.0); + + if (!ctrl_node->has_parameter("param3")) + { + ctrl_node->declare_parameter("param3", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param3").as_double(), has_param_3 ? 3.0 : -10.0); + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + + auto wildcard_ctrl_3 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(wildcard_ctrl_3.info.name, "wildcard_ctrl_3"); + ASSERT_EQ(wildcard_ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + wildcard_ctrl_3.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true); + + auto wildcard_ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(wildcard_ctrl_2.info.name, "wildcard_ctrl_2"); + ASSERT_EQ(wildcard_ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + wildcard_ctrl_2.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_2.c->get_node(), false); + + auto wildcard_ctrl_1 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(wildcard_ctrl_1.info.name, "wildcard_ctrl_1"); + ASSERT_EQ(wildcard_ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + wildcard_ctrl_1.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false); +} + +TEST_F(TestLoadController, spawner_test_failed_activation_of_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_interfaces.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_command_interface ctrl_with_joint2_command_interface -c " + "test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_joint2_command_interface = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_joint2_command_interface.info.name, "ctrl_with_joint2_command_interface"); + ASSERT_EQ( + ctrl_with_joint2_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ( + ctrl_with_joint2_command_interface.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_with_joint2_command_interface.c->command_interface_configuration().names.size(), 1ul); + ASSERT_THAT( + ctrl_with_joint2_command_interface.c->command_interface_configuration().names, + std::vector({"joint2/velocity"})); + + auto ctrl_with_joint1_command_interface = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_with_joint1_command_interface.info.name, "ctrl_with_joint1_command_interface"); + ASSERT_EQ( + ctrl_with_joint1_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ( + ctrl_with_joint1_command_interface.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_with_joint1_command_interface.c->command_interface_configuration().names.size(), 1ul); + ASSERT_THAT( + ctrl_with_joint1_command_interface.c->command_interface_configuration().names, + std::vector({"joint1/position"})); + + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 256) + << "Should fail as the ctrl_with_joint1_command_interface and " + "ctrl_with_joint2_command_interface are active"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + + auto ctrl_with_joint1_and_joint2_command_interfaces = cm_->get_loaded_controllers()[0]; + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.info.name, + "ctrl_with_joint1_and_joint2_command_interfaces"); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.info.type, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration() + .names.size(), + 2ul); + ASSERT_THAT( + ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration().names, + std::vector({"joint1/position", "joint2/velocity"})); + + EXPECT_EQ(call_unspawner("ctrl_with_joint1_command_interface -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 256) + << "Should fail as the ctrl_with_joint2_command_interface is still active"; + + EXPECT_EQ(call_unspawner("ctrl_with_joint2_command_interface -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0) + << "Should pass as the ctrl_with_joint1_command_interface and " + "ctrl_with_joint2_command_interface are inactive"; +} + TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill @@ -357,6 +574,114 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint10"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "impedance") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + TEST_F(TestLoadController, spawner_test_fallback_controllers) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + @@ -377,7 +702,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); } // Try to spawn now the controller with fallback controllers inside the yaml @@ -392,7 +717,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); @@ -401,7 +726,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string_array()[0], test_file_path); auto ctrl_3 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); @@ -409,7 +734,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); ASSERT_EQ( ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string_array()[0], test_file_path); } } @@ -439,6 +764,45 @@ TEST_F(TestLoadController, spawner_with_many_controllers) } } +TEST_F(TestLoadController, test_spawner_parsed_controller_ros_args) +{ + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + // Now as the controller is active, we can call check for the service + std::shared_ptr node = rclcpp::Node::make_shared("set_bool_client"); + auto set_bool_service = node->create_client("/set_bool"); + ASSERT_FALSE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_1_set_bool_service = + node->create_client("/ctrl_1/set_bool"); + ASSERT_TRUE(ctrl_1_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(ctrl_1_set_bool_service->service_is_ready()); + + // Now test the remapping of the service name with the controller_ros_args + EXPECT_EQ( + call_spawner( + "ctrl_2 -c test_controller_manager --controller-ros-args '-r /ctrl_2/set_bool:=/set_bool'"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + // Now as the controller is active, we can call check for the remapped service + ASSERT_TRUE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_2_set_bool_service = + node->create_client("/ctrl_2/set_bool"); + ASSERT_FALSE(ctrl_2_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(ctrl_2_set_bool_service->service_is_ready()); +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { @@ -695,7 +1059,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -708,7 +1073,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); EXPECT_EQ( @@ -725,14 +1091,16 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } TEST_F( @@ -781,7 +1149,8 @@ TEST_F( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -794,7 +1163,8 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); EXPECT_EQ( @@ -812,14 +1182,16 @@ TEST_F( ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) @@ -928,3 +1300,138 @@ TEST_F( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } + +TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " -p" + fallback_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + auto params_file_info = + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); + + auto ctrl_1 = cm_->get_loaded_controllers()[3]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +} + +TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " -p" + fallback_test_file_path + " -p" + fallback_test_file_path + " -p" + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + auto params_file_info = + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); + + auto ctrl_1 = cm_->get_loaded_controllers()[3]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +} diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 46ecc7ab6e..fc92596273 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + +4.21.0 (2024-12-06) +------------------- + 4.20.0 (2024-11-08) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 78a9f99591..703feeccc7 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.20.0 + 4.22.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 09a9236d79..69c282b051 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -76,11 +76,15 @@ controller_manager * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* The spawner now supports the ``--controller-ros-args`` argument to pass the ROS 2 node arguments to the controller node to be able to remap topics, services and etc (`#1713 `_). +* The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). +* The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). +* The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). hardware_interface ****************** @@ -111,10 +115,49 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* The ``ros2_control`` tag now supports parsing read/write rate ``rw_rate`` for the each hardware component parsed through the URDF (`#1570 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. * With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. +* With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. joint_limits ************ diff --git a/downstream.humble.repos b/downstream.humble.repos new file mode 100644 index 0000000000..d4ee6ec81a --- /dev/null +++ b/downstream.humble.repos @@ -0,0 +1,13 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: humble + webots/webots_ros2: + type: git + url: https://github.com/cyberbotics/webots_ros2.git + version: master + PickNikRobotics/topic_based_ros2_control: + type: git + url: https://github.com/PickNikRobotics/topic_based_ros2_control.git + version: main diff --git a/downstream.jazzy.repos b/downstream.jazzy.repos new file mode 100644 index 0000000000..13f06e7b44 --- /dev/null +++ b/downstream.jazzy.repos @@ -0,0 +1,13 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main + webots/webots_ros2: + type: git + url: https://github.com/cyberbotics/webots_ros2.git + version: master + PickNikRobotics/topic_based_ros2_control: + type: git + url: https://github.com/PickNikRobotics/topic_based_ros2_control.git + version: main diff --git a/downstream.rolling.repos b/downstream.rolling.repos new file mode 100644 index 0000000000..13f06e7b44 --- /dev/null +++ b/downstream.rolling.repos @@ -0,0 +1,13 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main + webots/webots_ros2: + type: git + url: https://github.com/cyberbotics/webots_ros2.git + version: master + PickNikRobotics/topic_based_ros2_control: + type: git + url: https://github.com/PickNikRobotics/topic_based_ros2_control.git + version: main diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 17e276bc93..df05cb39e3 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Propagate read/write rate to the HardwareInfo properly (`#1928 `_) +* Async Hardware Components (`#1567 `_) +* Add controller node options args to be able to set controller specific node arguments (`#1713 `_) +* Make get_name() return a const reference (`#1952 `_) +* Let sensors also export state interfaces of joints (`#1885 `_) +* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Takashi Sato + +4.21.0 (2024-12-06) +------------------- +* [Feature] Choose different read and write rate for the hardware components (`#1570 `_) +* add logic for 'params_file' to handle both string and string_array (`#1898 `_) +* [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) +* Fix missing virtual of on_export\_[state|command]_interfaces methods (`#1888 `_) +* Refactor: add parse_state_interface_descriptions and parse_command_interface_descriptions to import the components (`#1768 `_) +* Contributors: Sai Kishor Kothakota, Takashi Sato + 4.20.0 (2024-11-08) ------------------- * Add Support for SDF (`#1763 `_) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 385ae96fb1..4e3fe6a9ae 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils rcutils + realtime_tools TinyXML2 tinyxml2_vendor joint_limits @@ -21,6 +22,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS find_package(ament_cmake REQUIRED) find_package(ament_cmake_gen_version_h REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/hardware_interface/doc/asynchronous_components.rst b/hardware_interface/doc/asynchronous_components.rst new file mode 100644 index 0000000000..c73281c3fe --- /dev/null +++ b/hardware_interface/doc/asynchronous_components.rst @@ -0,0 +1,99 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/different_update_rates_userdoc.rst + +.. _asynchronous_components: + +Running Hardware Components Asynchronously +============================================ + +The ``ros2_control`` framework allows to run hardware components asynchronously. This is useful when some of the hardware components need to run in a separate thread or executor. For example, a sensor takes longer to read data that affects the periodicity of the ``controller_manager`` control loop. In this case, the sensor can be run in a separate thread or executor to avoid blocking the control loop. + +Parameters +----------- + +The following parameters can be set in the ``ros2_control`` hardware configuration to run the hardware component asynchronously: + +* ``is_async``: (optional) If set to ``true``, the hardware component will run asynchronously. Default is ``false``. +* ``thread_priority``: (optional) The priority of the thread that runs the hardware component. The priority is an integer value between 0 and 99. The default value is 50. + +.. note:: + The thread priority is only used when the hardware component is run asynchronously. + When the hardware component is run asynchronously, it uses the FIFO scheduling policy. + +Examples +--------- + +The following examples show how to use the different hardware interface types synchronously and asynchronously with ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +For a RRBot with multimodal gripper and external sensor: + +.. code-block:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + + +In the above example, the following components are defined: + +* A system hardware component named ``RRBotSystemMutipleGPIOs`` with two joints and a GPIO component that runs synchronously. +* An actuator hardware component named ``MultimodalGripper`` with a joint that runs asynchronously with a thread priority of 30. +* A sensor hardware component named ``RRBotForceTorqueSensor2D`` with two sensors and a GPIO component that runs asynchronously with the default thread priority of 50. diff --git a/hardware_interface/doc/different_update_rates_userdoc.rst b/hardware_interface/doc/different_update_rates_userdoc.rst index 23f5e3564a..5a71587d44 100644 --- a/hardware_interface/doc/different_update_rates_userdoc.rst +++ b/hardware_interface/doc/different_update_rates_userdoc.rst @@ -5,165 +5,86 @@ Different update rates for Hardware Components =============================================================================== -In the following sections you can find some advice which will help you to implement Hardware -Components with update rates different from the main control loop. - -By counting loops -------------------------------------------------------------------------------- - -Current implementation of -`ros2_control main node `_ -has one update rate that controls the rate of the -`read(...) `_ -and `write(...) `_ -calls in `hardware_interface(s) `_. -To achieve different update rates for your hardware component you can use the following steps: - -.. _step-1: - -1. Add parameters of main control loop update rate and desired update rate for your hardware component - - .. code:: xml - - - - - - - - - my_system_interface/MySystemHardware - ${main_loop_update_rate} - ${desired_hw_update_rate} - - ... - - - - - - -.. _step-2: - -2. In you ``on_init()`` specific implementation fetch the desired parameters - - .. code:: cpp - - namespace my_system_interface - { - hardware_interface::CallbackReturn MySystemHardware::on_init( - const hardware_interface::HardwareInfo & info) - { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - // declaration in *.hpp file --> unsigned int main_loop_update_rate_, desired_hw_update_rate_ = 100 ; - main_loop_update_rate_ = stoi(info_.hardware_parameters["main_loop_update_rate"]); - desired_hw_update_rate_ = stoi(info_.hardware_parameters["desired_hw_update_rate"]); - - ... - } - ... - } // my_system_interface - -3. In your ``on_activate`` specific implementation reset internal loop counter - - .. code:: cpp - - hardware_interface::CallbackReturn MySystemHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // declaration in *.hpp file --> unsigned int update_loop_counter_ ; - update_loop_counter_ = 0; - ... - } - -4. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` - and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)`` - specific implementations decide if you should interfere with your hardware - - .. code:: cpp - - hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) - { - - bool hardware_go = main_loop_update_rate_ == 0 || - desired_hw_update_rate_ == 0 || - ((update_loop_counter_ % desired_hw_update_rate_) == 0); - - if (hardware_go){ - // hardware comms and operations - ... - } - ... - - // update counter - ++update_loop_counter_; - update_loop_counter_ %= main_loop_update_rate_; - } - -By measuring elapsed time -------------------------------------------------------------------------------- - -Another way to decide if hardware communication should be executed in the -``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` and/or -``write(const rclcpp::Time & time, const rclcpp::Duration & period)`` -implementations is to measure elapsed time since last pass: - -1. In your ``on_activate`` specific implementation reset the flags that indicate - that this is the first pass of the ``read`` and ``write`` methods - - .. code:: cpp - - hardware_interface::CallbackReturn MySystemHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // declaration in *.hpp file --> bool first_read_pass_, first_write_pass_ = true ; - first_read_pass_ = first_write_pass_ = true; - ... - } - -2. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` - and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)`` - specific implementations decide if you should interfere with your hardware - - .. code:: cpp - - hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) - { - if (first_read_pass_ || (time - last_read_time_ ) > desired_hw_update_period_) - { - first_read_pass_ = false; - // declaration in *.hpp file --> rclcpp::Time last_read_time_ ; - last_read_time_ = time; - // hardware comms and operations - ... - } - ... - } - - hardware_interface::return_type MySystemHardware::write(const rclcpp::Time & time, const rclcpp::Duration & period) - { - if (first_write_pass_ || (time - last_write_time_ ) > desired_hw_update_period_) - { - first_write_pass_ = false; - // declaration in *.hpp file --> rclcpp::Time last_write_time_ ; - last_write_time_ = time; - // hardware comms and operations - ... - } - ... - } +The ``ros2_control`` framework allows to run different hardware components at different update rates. This is useful when some of the hardware components needs to run at a different frequency than the traditional control loop frequency which is same as the one of the ``controller_manager``. It is very typical to have different components with different frequencies in a robotic system with different sensors or different components using different communication protocols. +This is useful when you have a hardware component that needs to run at a higher rate than the rest of the components. For example, you might have a sensor that needs to be read at 1000Hz, while the rest of the components can run at 500Hz, given that the control loop frequency of the ``controller_manager`` is higher than 1000Hz. The read/write rate can be defined easily by adding the parameter ``rw_rate`` to the ``ros2_control`` tag of the hardware component. + +Examples +***************************** +The following examples show how to use the different hardware interface types with different update frequencies in a ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +For a RRBot with Multimodal gripper and external sensor running at different rates: + +.. code-block:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + + +In the above example, the system hardware component that controls the joints of the RRBot is running at 500 Hz, the multimodal gripper is running at 200 Hz and the force torque sensor is running at 250 Hz. .. note:: - - The approach to get the desired update period value from the URDF and assign it to the variable - ``desired_hw_update_period_`` is the same as in the previous section (|step-1|_ and |step-2|_) but - with a different parameter name. - -.. |step-1| replace:: step 1 -.. |step-2| replace:: step 2 + In the above example, the ``rw_rate`` parameter is set to 500 Hz, 200 Hz and 250 Hz for the system, actuator and sensor hardware components respectively. This parameter is optional and if not set, the default value of 0 will be used which means that the hardware component will run at the same rate as the ``controller_manager``. However, if the specified rate is higher than the ``controller_manager`` rate, the hardware component will then run at the rate of the ``controller_manager``. diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 0a980cad4e..3870433ef0 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -17,6 +17,7 @@ Guidelines and Best Practices Hardware Interface Types Writing a Hardware Component Different Update Rates + Asynchronous Execution Handling of errors that happen during read() and write() calls diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3b16a65261..bfeb5d969d 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -95,15 +95,28 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_lifecycle_state() const; + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_write_time() const; + HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; mutable std::recursive_mutex actuators_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8aa214e728..6f4dfa4402 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -27,6 +27,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" +#include "hardware_interface/types/trigger_type.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" @@ -34,6 +35,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +113,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod clock_interface_ = clock_interface; actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -321,6 +347,50 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return return_type::OK; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -333,6 +403,49 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -426,6 +539,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::vector unlisted_commands_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; @@ -433,6 +547,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index a38fb99cb3..d814ca7ae2 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,13 +34,16 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional parameters_file; + std::vector parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; /// List of fallback controller names to be activated if this controller fails. std::vector fallback_controllers_names; + + /// Controller node options arguments + std::vector node_options_args; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 092ed21000..70a0482811 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -22,6 +22,7 @@ #include #include +#include #include "rclcpp_lifecycle/state.hpp" namespace hardware_interface @@ -47,6 +48,9 @@ struct HardwareComponentInfo /// Component is async bool is_async; + //// read/write rate + unsigned int rw_rate; + /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eea8b6ca8a..9d96190954 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -174,8 +174,12 @@ struct HardwareInfo std::string type; /// Hardware group to which the hardware belongs. std::string group; + /// Component's read and write rates in Hz. + unsigned int rw_rate; /// Component is async bool is_async; + /// Async thread priority + int thread_priority; /// Name of the pluginlib plugin of the hardware that will be loaded. std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 6013dea778..7bb4d3a0ef 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -81,7 +81,7 @@ class LoanedCommandInterface } } - const std::string get_name() const { return command_interface_.get_name(); } + const std::string & get_name() const { return command_interface_.get_name(); } const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 3ebc8c7ca0..27b3da813e 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -75,7 +75,7 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + const std::string & get_name() const { return state_interface_.get_name(); } const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5592fc492..cdde7550d3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -426,12 +426,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); - /// Deletes all async components from the resource storage - /** - * Needed to join the threads immediately after the control loop is ended. - */ - void shutdown_async_components(); - /// Reads all loaded hardware components. /** * Reads from all active hardware components. diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index ca570b78aa..ac7f3f6f6a 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -82,15 +82,23 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_lifecycle_state() const; + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + HARDWARE_INTERFACE_PUBLIC 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; } + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; mutable std::recursive_mutex sensors_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 74c1d04bd7..93de2a6494 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -34,6 +34,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +112,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + read_async_handler_ = std::make_unique>(); + read_async_handler_->init( + std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2), + info_.thread_priority); + read_async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -123,6 +134,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); return CallbackReturn::SUCCESS; }; @@ -179,7 +191,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector state_interfaces; state_interfaces.reserve( - unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); + unlisted_interface_descriptions.size() + sensor_state_interfaces_.size() + + joint_state_interfaces_.size()); // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps. for (const auto & description : unlisted_interface_descriptions) @@ -201,9 +214,51 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.push_back(std::const_pointer_cast(state_interface)); } + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + return state_interfaces; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while read async handler is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -274,14 +329,17 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; std::unordered_map sensor_state_interfaces_; std::unordered_map unlisted_state_interfaces_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; std::vector sensor_states_; std::vector unlisted_states_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> read_async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 09adaa7190..749e10d5e4 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -95,15 +95,28 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_lifecycle_state() const; + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_write_time() const; + HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; mutable std::recursive_mutex system_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 18da0e4012..e145deae5b 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -28,6 +28,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" +#include "hardware_interface/types/trigger_type.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" @@ -36,6 +37,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -114,6 +116,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -178,7 +204,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - std::vector on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = @@ -270,7 +296,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored CommandInterfaces */ - std::vector on_export_command_interfaces() + virtual std::vector on_export_command_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = @@ -350,6 +376,50 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return return_type::OK; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -362,6 +432,49 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -453,6 +566,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. std::vector joint_states_; @@ -472,6 +586,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/trigger_type.hpp b/hardware_interface/include/hardware_interface/types/trigger_type.hpp new file mode 100644 index 0000000000..63970bc0f0 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/trigger_type.hpp @@ -0,0 +1,28 @@ +// Copyright 2024 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__TYPES__TRIGGER_TYPE_HPP_ +#define HARDWARE_INTERFACE__TYPES__TRIGGER_TYPE_HPP_ + +namespace hardware_interface +{ +enum class TriggerType +{ + READ, + WRITE +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__TRIGGER_TYPE_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index dbe9ad5750..5f748e5680 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.20.0 + 4.22.0 ros2_control hardware interface Bence Magyar Denis Štogl @@ -10,11 +10,13 @@ ament_cmake ament_cmake_gen_version_h + backward_ros control_msgs lifecycle_msgs pluginlib rclcpp_lifecycle rcpputils + realtime_tools tinyxml2_vendor joint_limits urdf diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index b0ed1f7c80..81bebf1182 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -41,6 +41,8 @@ Actuator::Actuator(Actuator && other) noexcept { std::lock_guard lock(other.actuators_mutex_); impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; + last_write_cycle_time_ = other.last_write_cycle_time_; } const rclcpp_lifecycle::State & Actuator::initialize( @@ -53,6 +55,8 @@ const rclcpp_lifecycle::State & Actuator::initialize( switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_write_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -282,18 +286,15 @@ const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +const rclcpp::Time & Actuator::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } + return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(actuators_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_read_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -301,27 +302,21 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); } + last_read_cycle_time_ = time; } return result; } return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(actuators_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_write_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -329,13 +324,15 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); } + last_write_cycle_time_ = time; } return result; } +std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0f186531e9..a3e9efaa3a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -58,7 +58,9 @@ constexpr const auto kTypeAttribute = "type"; constexpr const auto kRoleAttribute = "role"; constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; +constexpr const auto kReadWriteRateAttribute = "rw_rate"; constexpr const auto kIsAsyncAttribute = "is_async"; +constexpr const auto kThreadPriorityAttribute = "thread_priority"; } // namespace @@ -186,7 +188,7 @@ std::size_t parse_size_attribute(const tinyxml2::XMLElement * elem) std::regex int_re("[1-9][0-9]*"); if (std::regex_match(s, int_re)) { - size = std::stoi(s); + size = static_cast(std::stoi(s)); } else { @@ -222,6 +224,42 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) return data_type; } +/// Parse rw_rate attribute +/** + * Parses an XMLElement and returns the value of the rw_rate attribute. + * Defaults to 0 if not specified. + * + * \param[in] elem XMLElement that has the rw_rate attribute. + * \return unsigned int specifying the read/write rate. + */ +unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute); + try + { + const auto rw_rate = attr ? std::stoi(attr->Value()) : 0; + if (rw_rate < 0) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + std::to_string(rw_rate) + "\", but expected a positive integer."); + } + return static_cast(rw_rate); + } + catch (const std::invalid_argument & e) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + + " Invalid value : \"" + attr->Value() + "\", expected a positive integer."); + } + catch (const std::out_of_range & e) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + + " Out of range value : \"" + attr->Value() + "\", expected a positive valid integer."); + } +} + /// Parse is_async attribute /** * Parses an XMLElement and returns the value of the is_async attribute. @@ -236,6 +274,35 @@ bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) return attr ? parse_bool(attr->Value()) : false; } +/// Parse thread_priority attribute +/** + * Parses an XMLElement and returns the value of the thread_priority attribute. + * Defaults to 50 if not specified. + * + * \param[in] elem XMLElement that has the thread_priority attribute. + * \return positive integer specifying the thread priority. + */ +int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kThreadPriorityAttribute); + if (!attr) + { + return 50; + } + std::string s = attr->Value(); + std::regex int_re("[1-9][0-9]*"); + if (std::regex_match(s, int_re)) + { + return std::stoi(s); + } + else + { + throw std::runtime_error( + "Could not parse thread_priority tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + s + "\", but expected a non-zero positive integer."); + } +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -575,7 +642,10 @@ HardwareInfo parse_resource_from_xml( HardwareInfo hardware; hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag); hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); + hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it); hardware.is_async = parse_is_async_attribute(ros2_control_it); + hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it) + : std::numeric_limits::max(); // Parse everything under ros2_control tag hardware.hardware_plugin_name = ""; @@ -885,7 +955,7 @@ std::vector parse_control_resources_from_urdf(const std::string & { throw std::runtime_error("Mimic joint '" + name + "' not found in tag"); } - return std::distance(hw_info.joints.begin(), it); + return static_cast(std::distance(hw_info.joints.begin(), it)); }; MimicJoint mimic_joint; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 8b46d21f4d..2b1d635c97 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -117,7 +117,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // it's extremely improbable that std::distance results int this value - therefore default + // it's extremely improbable that std::distance results in this value - therefore default index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces @@ -157,7 +157,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i if (if_it != other_interfaces_.end()) { index_custom_interface_with_following_offset_ = - std::distance(other_interfaces_.begin(), if_it); + static_cast(std::distance(other_interfaces_.begin(), if_it)); RCLCPP_INFO( get_logger(), "Custom interface with following offset '%s' found at index: %zu.", custom_interface_with_following_offset_.c_str(), @@ -348,7 +348,8 @@ return_type GenericSystem::prepare_command_mode_switch( if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); + const size_t joint_index = + static_cast(std::distance(info.joints.begin(), joint_it_found)); if (joint_found_in_x_requests_[joint_index] == 0) { joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; @@ -436,7 +437,8 @@ return_type GenericSystem::perform_command_mode_switch( if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); + const size_t joint_index = + static_cast(std::distance(info.joints.begin(), joint_it_found)); if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { @@ -630,7 +632,7 @@ bool GenericSystem::get_interface( auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); if (it != interface_list.end()) { - auto j = std::distance(interface_list.begin(), it); + auto j = static_cast(std::distance(interface_list.begin(), it)); interfaces.emplace_back(name, *it, &values[j][vector_index]); return true; } @@ -662,7 +664,7 @@ void GenericSystem::initialize_storage_vectors( // If interface name is found in the interfaces list if (it != interfaces.end()) { - auto index = std::distance(interfaces.begin(), it); + auto index = static_cast(std::distance(interfaces.begin(), it)); // Check the initial_value param is used if (!interface.initial_value.empty()) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e5445491c8..7a616d890d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -26,7 +26,6 @@ #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" @@ -142,6 +141,7 @@ class ResourceStorage component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.group = hardware_info.group; + component_info.rw_rate = hardware_info.rw_rate; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; @@ -298,15 +298,6 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } - - if (hardware_info_map_[hardware.get_name()].is_async) - { - async_component_threads_.emplace( - std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), - std::forward_as_tuple(cm_update_rate_, clock_interface_)); - - async_component_threads_.at(hardware.get_name()).register_component(&hardware); - } } if (!hardware.get_group_name().empty()) { @@ -427,7 +418,6 @@ class ResourceStorage if (result) { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); - async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); @@ -462,16 +452,6 @@ class ResourceStorage get_logger(), "Unknown exception occurred while activating hardware '%s'", hardware.get_name().c_str()); } - - if (result) - { - if (async_component_threads_.find(hardware.get_name()) != async_component_threads_.end()) - { - async_component_threads_.at(hardware.get_name()).activate(); - } - // TODO(destogl): make all command interfaces available (currently are all available) - } - return result; } @@ -611,11 +591,10 @@ class ResourceStorage auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); - RCLCPP_WARN( - get_logger(), + RCLCPP_WARN_EXPRESSION( + get_logger(), interface_names.empty(), "Importing state interfaces for the hardware '%s' returned no state interfaces.", hardware.get_name().c_str()); - hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); @@ -824,15 +803,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_actuators(async_actuators_); - } - else - { - return load_and_init_actuators(actuators_); - } + return load_and_init_actuators(actuators_); } bool load_and_initialize_sensor(const HardwareInfo & hardware_info) @@ -857,14 +828,7 @@ class ResourceStorage return true; }; - if (hardware_info.is_async) - { - return load_and_init_sensors(async_sensors_); - } - else - { - return load_and_init_sensors(sensors_); - } + return load_and_init_sensors(sensors_); } bool load_and_initialize_system(const HardwareInfo & hardware_info) @@ -889,15 +853,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_systems(async_systems_); - } - else - { - return load_and_init_systems(systems_); - } + return load_and_init_systems(systems_); } void initialize_actuator( @@ -919,14 +875,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_actuators(async_actuators_); - } - else - { - init_actuators(actuators_); - } + init_actuators(actuators_); } void initialize_sensor( @@ -947,14 +896,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_sensors(async_sensors_); - } - else - { - init_sensors(sensors_); - } + init_sensors(sensors_); } void initialize_system( @@ -976,14 +918,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_systems(async_systems_); - } - else - { - init_systems(systems_); - } + init_systems(systems_); } void clear() @@ -992,10 +927,6 @@ class ResourceStorage sensors_.clear(); systems_.clear(); - async_actuators_.clear(); - async_sensors_.clear(); - async_systems_.clear(); - hardware_info_map_.clear(); state_interface_map_.clear(); command_interface_map_.clear(); @@ -1053,10 +984,6 @@ class ResourceStorage std::vector sensors_; std::vector systems_; - std::vector async_actuators_; - std::vector async_sensors_; - std::vector async_systems_; - std::unordered_map hardware_info_map_; std::unordered_map hw_group_state_; @@ -1080,9 +1007,6 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; - /// List of async components by type - std::unordered_map async_component_threads_; - // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1124,7 +1048,12 @@ bool ResourceManager::load_and_initialize_components( resource_storage_->cm_update_rate_ = update_rate; - const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + // Set the update rate for all hardware components + for (auto & hw : hardware_info) + { + hw.rw_rate = (hw.rw_rate == 0 || hw.rw_rate > update_rate) ? update_rate : hw.rw_rate; + } const std::string system_type = "system"; const std::string sensor_type = "sensor"; @@ -1524,11 +1453,8 @@ std::unordered_map ResourceManager::get_comp }; loop_and_get_state(resource_storage_->actuators_); - loop_and_get_state(resource_storage_->async_actuators_); loop_and_get_state(resource_storage_->sensors_); - loop_and_get_state(resource_storage_->async_sensors_); loop_and_get_state(resource_storage_->systems_); - loop_and_get_state(resource_storage_->async_systems_); return resource_storage_->hardware_info_map_; } @@ -1796,35 +1722,10 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_actuators_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_systems_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_sensors_); - } return result; } -void ResourceManager::shutdown_async_components() -{ - resource_storage_->async_component_threads_.erase( - resource_storage_->async_component_threads_.begin(), - resource_storage_->async_component_threads_.end()); -} - // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) @@ -1836,10 +1737,35 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping read() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } auto ret_val = return_type::OK; try { - ret_val = component.read(time, period); + if ( + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == + resource_storage_->cm_update_rate_) + { + ret_val = component.read(time, period); + } + else + { + const double read_rate = + resource_storage_->hardware_info_map_[component.get_name()].rw_rate; + const auto current_time = resource_storage_->get_clock()->now(); + const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); + if (actual_period.seconds() * read_rate >= 0.99) + { + ret_val = component.read(current_time, actual_period); + } + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); @@ -1897,10 +1823,35 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping write() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } auto ret_val = return_type::OK; try { - ret_val = component.write(time, period); + if ( + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == + resource_storage_->cm_update_rate_) + { + ret_val = component.write(time, period); + } + else + { + const double write_rate = + resource_storage_->hardware_info_map_[component.get_name()].rw_rate; + const auto current_time = resource_storage_->get_clock()->now(); + const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); + if (actual_period.seconds() * write_rate >= 0.99) + { + ret_val = component.write(current_time, actual_period); + } + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 5da01368c9..ae4d930bbe 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -40,6 +40,7 @@ Sensor::Sensor(Sensor && other) noexcept { std::lock_guard lock(other.sensors_mutex_); impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; } const rclcpp_lifecycle::State & Sensor::initialize( @@ -52,6 +53,7 @@ const rclcpp_lifecycle::State & Sensor::initialize( switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -240,18 +242,13 @@ const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } + return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(sensors_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_read_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -259,13 +256,15 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); } + last_read_cycle_time_ = time; } return result; } +std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index abae924dfc..89d6afd42e 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -39,6 +39,8 @@ System::System(System && other) noexcept { std::lock_guard lock(other.system_mutex_); impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; + last_write_cycle_time_ = other.last_write_cycle_time_; } const rclcpp_lifecycle::State & System::initialize( @@ -51,6 +53,8 @@ const rclcpp_lifecycle::State & System::initialize( switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_write_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -280,18 +284,15 @@ const rclcpp_lifecycle::State & System::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } + return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(system_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for system '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_read_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -299,27 +300,21 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); } + last_read_cycle_time_ = time; } return result; } return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(system_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping write() call for system '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_write_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -327,13 +322,15 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); } + last_write_cycle_time_ = time; } return result; } +std::recursive_mutex & System::get_mutex() { return system_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 854b35c8d3..446634d12f 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -699,9 +699,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager } explicit TestableResourceManager( - rclcpp::Node & node, const std::string & urdf, bool activate_all = false) + rclcpp::Node & node, const std::string & urdf, bool activate_all = false, + unsigned int cm_update_rate = 100) : hardware_interface::ResourceManager( - urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) + urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, + cm_update_rate) { } }; @@ -842,14 +844,17 @@ void generic_system_functional_test( EXPECT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); + EXPECT_EQ(status_map[component_name].rw_rate, 100u); configure_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ(status_map[component_name].rw_rate, 100u); activate_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ(status_map[component_name].rw_rate, 100u); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -935,7 +940,7 @@ void generic_system_error_group_test( const std::string & urdf, const std::string component_prefix, bool validate_same_group) { rclcpp::Node node("test_generic_system"); - TestableResourceManager rm(node, urdf); + TestableResourceManager rm(node, urdf, false, 200u); const std::string component1 = component_prefix + "1"; const std::string component2 = component_prefix + "2"; // check is hardware is configured @@ -944,14 +949,17 @@ void generic_system_error_group_test( { EXPECT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); + EXPECT_EQ(status_map[component].rw_rate, 200u); configure_components(rm, {component}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ(status_map[component].rw_rate, 200u); activate_components(rm, {component}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ(status_map[component].rw_rate, 200u); } // Check initial values diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 9b90d81dfd..28a74aeb66 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -33,6 +33,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -270,7 +271,7 @@ class DummySensor : public hardware_interface::SensorInterface // We can read some voltage level std::vector state_interfaces; state_interfaces.emplace_back( - hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); + hardware_interface::StateInterface("sens1", "voltage", &voltage_level_)); return state_interfaces; } @@ -331,15 +332,72 @@ class DummySensorDefault : public hardware_interface::SensorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - for (const auto & [name, descr] : sensor_state_interfaces_) + set_state("sens1/voltage", 0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("sens1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySensorJointDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - set_state(name, 0.0); + return hardware_interface::CallbackReturn::ERROR; } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 10.0); + set_state("sens1/voltage", 0.0); read_calls_ = 0; return CallbackReturn::SUCCESS; } - std::string get_name() const override { return "DummySensorDefault"; } + std::string get_name() const override { return "DummySensorJointDefault"; } hardware_interface::return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -351,7 +409,8 @@ class DummySensorDefault : public hardware_interface::SensorInterface } // no-op, static value - set_state("joint1/voltage", voltage_level_hw_value_); + set_state("joint1/position", position_hw_value_); + set_state("sens1/voltage", voltage_level_hw_value_); return hardware_interface::return_type::OK; } @@ -370,6 +429,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface } private: + double position_hw_value_ = 0x777; double voltage_level_hw_value_ = 0x666; // Helper variables to initiate error on read @@ -466,7 +526,7 @@ class DummySystem : public hardware_interface::SystemInterface return hardware_interface::return_type::ERROR; } - for (auto i = 0; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { position_state_[i] += velocity_command_[0]; velocity_state_[i] = velocity_command_[0]; @@ -476,7 +536,7 @@ class DummySystem : public hardware_interface::SystemInterface CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override { - for (auto i = 0ul; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { velocity_state_[i] = 0.0; } @@ -572,7 +632,7 @@ class DummySystemDefault : public hardware_interface::SystemInterface return hardware_interface::return_type::ERROR; } - for (auto i = 0; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { auto current_pos = get_state(position_states_[i]); set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); @@ -683,8 +743,9 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -781,8 +842,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -896,16 +958,17 @@ TEST(TestComponentInterfaces, dummy_sensor) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); + auto state = + sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("sens1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[0]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Not updated because is is UNCONFIGURED @@ -935,37 +998,92 @@ TEST(TestComponentInterfaces, dummy_sensor_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/voltage"); + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); EXPECT_TRUE(contains); - EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } // Not updated because is is UNCONFIGURED - auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; + auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_joint) +{ + hardware_interface::Sensor sensor_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_joint_voltage_sensor + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo sensor_res = control_resources[0]; + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(sensor_res, node->get_logger(), node->get_node_clock_interface()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + + auto [contains_sens1_vol, si_sens1_vol] = + test_components::vector_contains(state_interfaces, "sens1/voltage"); + ASSERT_TRUE(contains_sens1_vol); + EXPECT_EQ("sens1/voltage", state_interfaces[si_sens1_vol]->get_name()); + EXPECT_EQ("voltage", state_interfaces[si_sens1_vol]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[si_sens1_vol]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + + auto [contains_joint1_pos, si_joint1_pos] = + test_components::vector_contains(state_interfaces, "joint1/position"); + ASSERT_TRUE(contains_joint1_pos); + EXPECT_EQ("joint1/position", state_interfaces[si_joint1_pos]->get_name()); + EXPECT_EQ("position", state_interfaces[si_joint1_pos]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[si_joint1_pos]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value()); } // BEGIN (Handle export change): for backward compatibility @@ -974,8 +1092,9 @@ TEST(TestComponentInterfaces, dummy_system) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1106,8 +1225,9 @@ TEST(TestComponentInterfaces, dummy_system_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1300,8 +1420,9 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) hardware_interface::System system_hw( std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1333,8 +1454,9 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1401,8 +1523,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1466,8 +1589,9 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1534,8 +1658,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1598,8 +1723,9 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); + auto state = + sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1670,8 +1796,9 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1694,7 +1821,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values - auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; state = sensor_hw.configure(); EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); @@ -1725,8 +1852,9 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1797,8 +1925,9 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1863,8 +1992,9 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1935,8 +2065,9 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1994,3 +2125,10 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index ab6f490b92..62e2b703cf 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -33,6 +33,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -169,8 +170,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_component"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -234,29 +236,29 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_component"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/voltage"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); + ASSERT_TRUE(contains); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } { auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + test_components::vector_contains(state_interfaces, "sens1/some_unlisted_interface"); + ASSERT_TRUE(contains); + EXPECT_EQ("sens1/some_unlisted_interface", state_interfaces[position]->get_name()); EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); } } @@ -271,8 +273,9 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_component"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -373,3 +376,10 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); } } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 3c24b0cc2a..e1a657e110 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -879,6 +879,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); ASSERT_THAT(hardware_info.joints, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); @@ -949,6 +951,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); @@ -1337,6 +1341,59 @@ TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limit EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) +{ + std::string urdf_to_test = ros2_control_test_assets::minimal_async_robot_urdf; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(3)); + auto hardware_info = control_hardware[0]; + + // Actuator + EXPECT_EQ(hardware_info.name, "TestActuatorHardware"); + EXPECT_EQ(hardware_info.type, "actuator"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 30); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + // Sensor + hardware_info = control_hardware[1]; + EXPECT_EQ(hardware_info.name, "TestSensorHardware"); + EXPECT_EQ(hardware_info.type, "sensor"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, IsEmpty()); + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 50); + + EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); + EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(1)); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty()); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); + + // System + hardware_info = control_hardware[2]; + EXPECT_EQ(hardware_info.name, "TestSystemHardware"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint2"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + EXPECT_EQ(hardware_info.joints[1].name, "joint3"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.gpios[0].name, "configuration"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 70); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -1361,6 +1418,8 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), ""); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + // when not set, rw_rate should be 0 + EXPECT_EQ(hardware_info.rw_rate, 0u); // Verify limits parsed from the URDF ASSERT_THAT(hardware_info.limits, SizeIs(1)); @@ -1377,6 +1436,16 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) } } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_invalid_thread_priority) +{ + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + "" + + std::string(ros2_control_test_assets::urdf_tail); + ; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + @@ -1425,6 +1494,8 @@ TEST_F(TestComponentParser, gripper_mimic_true_valid_config) EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); + // when not set, rw_rate should be 0 + EXPECT_EQ(hw_info[0].rw_rate, 0u); } TEST_F(TestComponentParser, gripper_no_mimic_valid_config) @@ -1441,6 +1512,70 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config) EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); + // when not set, rw_rate should be 0 + EXPECT_EQ(hw_info[0].rw_rate, 0u); +} + +TEST_F(TestComponentParser, negative_rw_rates_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_with_negative_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_rw_rates_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_invalid_with_text_in_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_rw_rates_out_of_range) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_invalid_out_of_range_in_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, valid_rw_rate) +{ + std::vector hw_info; + ASSERT_NO_THROW( + hw_info = parse_control_resources_from_urdf( + ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate)); + ASSERT_THAT(hw_info, SizeIs(3)); + EXPECT_EQ(hw_info[0].name, "TestActuatorHardware"); + EXPECT_EQ(hw_info[0].type, "actuator"); + EXPECT_EQ(hw_info[0].hardware_plugin_name, "test_actuator"); + ASSERT_THAT(hw_info[0].joints, SizeIs(1)); + EXPECT_EQ(hw_info[0].joints[0].name, "joint1"); + EXPECT_EQ(hw_info[0].rw_rate, 50u); + + EXPECT_EQ(hw_info[1].name, "TestSensorHardware"); + EXPECT_EQ(hw_info[1].type, "sensor"); + EXPECT_EQ(hw_info[1].hardware_plugin_name, "test_sensor"); + ASSERT_THAT(hw_info[1].sensors, SizeIs(1)); + EXPECT_EQ(hw_info[1].sensors[0].name, "sensor1"); + EXPECT_EQ(hw_info[1].rw_rate, 20u); + + EXPECT_EQ(hw_info[2].name, "TestSystemHardware"); + EXPECT_EQ(hw_info[2].type, "system"); + EXPECT_EQ(hw_info[2].hardware_plugin_name, "test_system"); + ASSERT_THAT(hw_info[2].joints, SizeIs(2)); + EXPECT_EQ(hw_info[2].joints[0].name, "joint2"); + EXPECT_EQ(hw_info[2].joints[1].name, "joint3"); + ASSERT_THAT(hw_info[2].gpios, SizeIs(1)); + EXPECT_EQ(hw_info[2].gpios[0].name, "configuration"); + EXPECT_EQ(hw_info[2].rw_rate, 25u); } TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) diff --git a/hardware_interface/test/test_macros.cpp b/hardware_interface/test/test_macros.cpp index 18e9e292c0..db62bce38f 100644 --- a/hardware_interface/test/test_macros.cpp +++ b/hardware_interface/test/test_macros.cpp @@ -62,7 +62,7 @@ TEST_F(TestMacros, throw_on_not_null) EXPECT_ANY_THROW(THROW_ON_NOT_NULLPTR(a_ptr)); std::vector vec_ptr; - for (auto i : {0, 1, 2}) + for (size_t i = 0; i < 3; ++i) { vec_ptr.push_back(i_ptr); EXPECT_ANY_THROW(THROW_ON_NOT_NULLPTR(vec_ptr[i])); diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 8daf6bab93..2bdadd883a 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Propagate read/write rate to the HardwareInfo properly (`#1928 `_) +* Async Hardware Components (`#1567 `_) +* Contributors: Sai Kishor Kothakota + +4.21.0 (2024-12-06) +------------------- +* [Feature] Choose different read and write rate for the hardware components (`#1570 `_) +* Contributors: Sai Kishor Kothakota + 4.20.0 (2024-11-08) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 3966cbc993..8c03aafda8 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.20.0 + 4.22.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 86debc1f4d..630cb9f27c 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -15,6 +15,7 @@ #include #include "hardware_interface/actuator_interface.hpp" +#include "rclcpp/logging.hpp" #include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; @@ -35,6 +36,14 @@ class TestActuator : public ActuatorInterface { return CallbackReturn::ERROR; } + if (get_hardware_info().rw_rate == 0u) + { + RCLCPP_WARN( + get_logger(), + "Actuator hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.", + get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); + return CallbackReturn::ERROR; + } /* * a hardware can optional prove for incorrect info here. diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 16692f55d9..40f27530c0 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -15,6 +15,7 @@ #include #include "hardware_interface/sensor_interface.hpp" +#include "rclcpp/logging.hpp" using hardware_interface::return_type; using hardware_interface::SensorInterface; @@ -33,6 +34,14 @@ class TestSensor : public SensorInterface { return CallbackReturn::ERROR; } + if (get_hardware_info().rw_rate == 0u) + { + RCLCPP_WARN( + get_logger(), + "Sensor hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.", + get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); + return CallbackReturn::ERROR; + } return CallbackReturn::SUCCESS; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index e30b74488e..395935e314 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -17,6 +17,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" #include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; @@ -39,6 +40,14 @@ class TestSystem : public SystemInterface return CallbackReturn::ERROR; } + if (get_hardware_info().rw_rate == 0u) + { + RCLCPP_WARN( + get_logger(), + "System hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.", + get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); + return CallbackReturn::ERROR; + } return CallbackReturn::SUCCESS; } @@ -104,6 +113,12 @@ class TestSystem : public SystemInterface { return return_type::DEACTIVATE; } + // The next line is for the testing purposes. We need value to be changed to + // be sure that the feedback from hardware to controllers in the chain is + // working as it should. This makes value checks clearer and confirms there + // is no "state = command" line or some other mixture of interfaces + // somewhere in the test stack. + velocity_state_[0] = velocity_command_[0] / 2.0; return return_type::OK; } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index e72a4a8214..be9e672b3b 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -444,7 +444,8 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) TEST_F(ResourceManagerTest, resource_status) { - TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm( + node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate); auto status_map = rm.get_components_status(); @@ -456,6 +457,10 @@ TEST_F(ResourceManagerTest, resource_status) EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE); + // read/write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].rw_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate, 25u); // plugin_name EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].plugin_name, TEST_ACTUATOR_HARDWARE_PLUGIN_NAME); @@ -1733,6 +1738,426 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) } } +class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate, false); + activate_components(*rm); + + cm_update_rate_ = 100u; // The default value inside + time = node_.get_clock()->now(); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // read/write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].rw_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate, 25u); + + actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate; + system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate; + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + using FunctionT = + std::function; + + void check_read_and_write_cycles(bool test_for_changing_values) + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + + for (size_t i = 1; i < 100; i++) + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + if (i % (cm_update_rate_ / system_rw_rate_) == 0 && test_for_changing_values) + { + // The values are computations exactly within the test_components + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + 20.0); + } + if (i % (cm_update_rate_ / actuator_rw_rate_) == 0 && test_for_changing_values) + { + // The values are computations exactly within the test_components + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); + } + // Even though we skip some read and write iterations, the state interfaces should be the same + // as previous updated one until the next cycle + ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value); + ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); + + // values to set to hardware to simulate failure on read and write +}; + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_deactivate) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_unconfigured) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_finalized) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + +class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + const auto minimal_robot_urdf_async = + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::async_hardware_resources) + + std::string(ros2_control_test_assets::urdf_tail); + rm = std::make_shared(node_, minimal_robot_urdf_async, false); + activate_components(*rm); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Check that all the components are async + ASSERT_TRUE(status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SYSTEM_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SENSOR_HARDWARE_NAME].is_async); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + // claimed_itfs[0].set_value(10.0); + // claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + void check_read_and_write_cycles(bool check_for_updated_values) + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + const double actuator_increment = 10.0; + const double system_increment = 20.0; + for (size_t i = 1; i < 100; i++) + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + // The values are computations exactly within the test_components + if (check_for_updated_values) + { + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + } + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + actuator_increment); + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + system_increment); + // This is needed to account for any missing hits to the read and write cycles as the tests + // are going to be run on a non-RT operating system + ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0); + ASSERT_NEAR(state_itfs[1].get_value(), prev_system_state_value, system_increment / 2.0); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + time = time + duration; + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); +}; + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_deactivate) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_unconfigured) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_finalized) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 6b00437dd1..1fa0b3a392 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + +4.21.0 (2024-12-06) +------------------- +* Use the .hpp headers from realtime_tools package (`#1916 `_) +* Contributors: Christoph Fröhlich + 4.20.0 (2024-11-08) ------------------- diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 42403d56ae..17bffed021 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -24,7 +24,7 @@ #include "joint_limits/visibility_control.h" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace joint_limits diff --git a/joint_limits/package.xml b/joint_limits/package.xml index c7fca5f2af..44ce8f5c50 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.20.0 + 4.22.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control.humble.repos b/ros2_control.humble.repos index 98b2866d38..af5cbc3c26 100644 --- a/ros2_control.humble.repos +++ b/ros2_control.humble.repos @@ -2,7 +2,7 @@ repositories: ros-controls/realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: humble ros-controls/control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 5b57ddc2e2..231aa395c6 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + +4.21.0 (2024-12-06) +------------------- + 4.20.0 (2024-11-08) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 925ad6e23f..7cc5192743 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.20.0 + 4.22.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 cad5325b2e..bfa3c5aae0 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Async Hardware Components (`#1567 `_) +* Let sensors also export state interfaces of joints (`#1885 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.21.0 (2024-12-06) +------------------- +* [Feature] Choose different read and write rate for the hardware components (`#1570 `_) +* Contributors: Sai Kishor Kothakota + 4.20.0 (2024-11-08) ------------------- * Add Support for SDF (`#1763 `_) 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 c887ca3251..46feee39a9 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 @@ -636,7 +636,24 @@ const auto valid_urdf_ros2_control_voltage_sensor_only = ros2_control_demo_hardware/SingleJointVoltageSensor 2 - + + + + +)"; + +// Joint+Voltage Sensor +const auto valid_urdf_ros2_control_joint_voltage_sensor = + R"( + + + ros2_control_demo_hardware/SingleJointVoltageSensor + 2 + + + + + diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index e5a4de8cab..0be4ed369b 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -662,7 +662,7 @@ const auto hardware_resources = const auto async_hardware_resources = R"( - + test_actuator @@ -683,7 +683,7 @@ const auto async_hardware_resources = - + test_system 2 @@ -709,6 +709,100 @@ const auto async_hardware_resources = )"; +const auto hardware_resources_with_different_rw_rates = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_with_negative_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + +const auto hardware_resources_invalid_with_text_in_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + +const auto hardware_resources_invalid_out_of_range_in_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + const auto uninitializable_hardware_resources = R"( @@ -1991,6 +2085,9 @@ const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_urdf_with_different_hw_rw_rate = + std::string(urdf_head) + std::string(hardware_resources_with_different_rw_rates) + + std::string(urdf_tail); const auto minimal_uninitializable_robot_urdf = std::string(urdf_head) + std::string(uninitializable_hardware_resources) + std::string(urdf_tail); diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index bb818063ab..27ecd415c7 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.20.0 + 4.22.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 894bbb8ea3..cb70f08797 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + +4.21.0 (2024-12-06) +------------------- +* [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) +* Contributors: Sai Kishor Kothakota + 4.20.0 (2024-11-08) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index ddb904c432..4c2a9cb99f 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.20.0 + 4.22.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index e47540a1df..5e29058a31 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -17,7 +17,7 @@ load_controller, list_controllers, switch_controllers, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) @@ -68,11 +68,11 @@ def main(self, *, args): if not os.path.isabs(args.param_file): args.param_file = os.path.join(os.getcwd(), args.param_file) - if not set_controller_parameters_from_param_file( + if not set_controller_parameters_from_param_files( node, args.controller_manager, args.controller_name, - args.param_file, + [args.param_file], node.get_namespace(), ): return 1 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 949445ec59..66c614b3ab 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.20.0", + version="4.22.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/ros_controls.humble.repos b/ros_controls.humble.repos new file mode 100644 index 0000000000..8cb447612b --- /dev/null +++ b/ros_controls.humble.repos @@ -0,0 +1,17 @@ +repositories: + ros-controls/gazebo_ros2_control: + type: git + url: https://github.com/ros-controls/gazebo_ros2_control.git + version: humble + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: humble + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: humble + ros-controls/ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: humble diff --git a/ros_controls.jazzy.repos b/ros_controls.jazzy.repos new file mode 100644 index 0000000000..d2c3a15743 --- /dev/null +++ b/ros_controls.jazzy.repos @@ -0,0 +1,13 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: jazzy + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master + ros-controls/ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: master diff --git a/ros_controls.rolling.repos b/ros_controls.rolling.repos new file mode 100644 index 0000000000..45d6fe6040 --- /dev/null +++ b/ros_controls.rolling.repos @@ -0,0 +1,13 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: rolling + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master + ros-controls/ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: master diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 1f9ad52f22..3ae6a8a682 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + +4.21.0 (2024-12-06) +------------------- + 4.20.0 (2024-11-08) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 1611ce05a3..cd944cf445 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.20.0 + 4.22.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 76d8c706c2..1d65fcef86 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.20.0", + version="4.22.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 842cc76488..6e94d7d826 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + +4.21.0 (2024-12-06) +------------------- + 4.20.0 (2024-11-08) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 7ae2f2eda7..035e6f0370 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.20.0 + 4.22.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