diff --git a/.github/dependabot.yml b/.github/dependabot.yml index f5e9921f23..f89baba15b 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -18,3 +18,17 @@ updates: schedule: interval: "weekly" target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "jazzy" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "kilted" diff --git a/.github/mergify.yml b/.github/mergify.yml index fd185e02d0..8293ec1b06 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,14 +8,23 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion + - name: Backport to jazzy at reviewers discretion conditions: - base=master - - "label=backport-iron" + - "label=backport-jazzy" actions: backport: branches: - - iron + - jazzy + + - name: Backport to kilted at reviewers discretion + conditions: + - base=master + - "label=backport-kilted" + actions: + backport: + branches: + - kilted - name: Ask to resolve conflict conditions: diff --git a/.github/workflows/README.md b/.github/workflows/README.md deleted file mode 100644 index 8e1845c5b9..0000000000 --- a/.github/workflows/README.md +++ /dev/null @@ -1,6 +0,0 @@ - -ROS2 Distro | Branch | Build status | Documentation | Released packages -:---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master)
[![Debian Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-debian-build.yml)
[![RHEL Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-rhel-binary-build.yml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[![Jazzy Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-source-build.yml?branch=master)
[![Debian Jazzy Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-debian-build.yml)
[![RHEL Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-rhel-binary-build.yml) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master)
[![Debian Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-debian-build.yml)
[![RHEL Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-rhel-binary-build.yml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 70366d4033..1cf5d72108 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -22,7 +22,7 @@ jobs: uses: docker/setup-buildx-action@v3 - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v5 - name: Log in to the Container registry uses: docker/login-action@v3 @@ -62,7 +62,7 @@ jobs: uses: docker/setup-buildx-action@v3 - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v5 - name: Log in to the Container registry uses: docker/login-action@v3 diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index f4172ee3e6..2240ef7b46 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -9,15 +9,16 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-abi-compatibility.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.humble.repos' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: abi_check: diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index 0fabfda92f..4437819647 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -4,19 +4,7 @@ name: Humble Binary Build on: workflow_dispatch: - pull_request: - branches: - - humble - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/humble-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control-not-released.humble.repos' - push: + pull_request: &event branches: - humble paths: @@ -24,18 +12,20 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.humble.repos' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: binary: diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index 078a541bf0..33483ffa55 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -8,7 +8,14 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/humble-check-docs.yml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index 8255b02bb4..68f1a48106 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -1,20 +1,7 @@ name: Coverage Build - Humble on: workflow_dispatch: - push: - branches: - - humble - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/humble-coverage-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.humble.repos' - - 'codecov.yml' - pull_request: + pull_request: &event branches: - humble paths: @@ -22,16 +9,18 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-coverage-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.humble.repos' - 'codecov.yml' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: coverage_humble: diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index d613d99ed3..d592d50444 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Humble Source Build +name: Humble - Debian Semi-Binary Build on: workflow_dispatch: pull_request: @@ -9,29 +9,26 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-debian-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - debian_source_build: + debian_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: master + ros_distro: humble + upstream_workspace: ros2_control.humble.repos + ref_for_scheduled_build: humble skip_packages: rqt_controller_manager skip_packages_test: controller_manager_msgs diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index 38a76ee025..e6cebafa83 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -5,6 +5,9 @@ on: pull_request: branches: - humble + push: + branches: + - humble concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/humble-pre-release.yml b/.github/workflows/humble-pre-release.yml new file mode 100644 index 0000000000..f54d97745d --- /dev/null +++ b/.github/workflows/humble-pre-release.yml @@ -0,0 +1,28 @@ +name: Humble - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - humble + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: humble + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} + prerelease_exclude_pkg: ackermann_steering_controller admittance_controller bicycle_steering_controller chained_filter_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster forward_command_controller gpio_controllers gps_sensor_broadcaster imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller motion_primitives_controllers omni_wheel_drive_controller parallel_gripper_controller pid_controller pose_broadcaster position_controllers range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes rqt_joint_trajectory_controller steering_controllers_library tricycle_controller tricycle_steering_controller velocity_controllers gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index e986bf361e..10b97eb5f2 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -1,4 +1,4 @@ -name: RHEL Humble Semi-Binary Build +name: Humble - RHEL Semi-Binary Build on: workflow_dispatch: pull_request: @@ -9,18 +9,19 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-rhel-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: rhel_semi_binary_build: diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 7b0c5f1fd3..d9e5c1f5ad 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -4,19 +4,7 @@ name: Humble Semi-Binary Build on: workflow_dispatch: - pull_request: - branches: - - humble - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/humble-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.humble.repos' - push: + pull_request: &event branches: - humble paths: @@ -24,18 +12,20 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.humble.repos' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: semi-binary: @@ -44,10 +34,9 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: humble semi-binary-clang: diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml index 117516b767..a1bc86a853 100644 --- a/.github/workflows/humble-semi-binary-downstream-build.yml +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -35,17 +35,3 @@ jobs: # 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 target_workspace, we just build it - not_test_build: true - # we don't test the downstream packages, which are outside of our organization - downstream_workspace: | # build also the ros2_control packages - ros_controls.humble.repos - downstream.humble.repos - not_test_downstream: true diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 878ad92677..8773e4c265 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -9,13 +9,19 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/humble-source-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.humble.repos' + pull_request: + branches: + - humble + paths: + - .github/workflows/humble-source-build.yml schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 3 * * MON-FRI' jobs: source: @@ -23,5 +29,4 @@ jobs: with: ros_distro: humble ref: humble - ros2_repo_branch: humble - os_name: ubuntu-22.04 + container: ubuntu:22.04 diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 83a81fec44..1fa362ccda 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -3,21 +3,22 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/jazzy-abi-compatibility.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.jazzy.repos' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: abi_check: diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index 599a075d9a..2d27459096 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -4,38 +4,28 @@ name: Jazzy Binary Build on: workflow_dispatch: - pull_request: + pull_request: &event branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/jazzy-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control-not-released.jazzy.repos' - push: - branches: - - master + - jazzy paths: - '**.hpp' - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/jazzy-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.jazzy.repos' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: binary: @@ -49,4 +39,4 @@ jobs: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_control-not-released.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index 7910cde0b5..2c85a88404 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -4,11 +4,18 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/jazzy-check-docs.yml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml index aa345d1e80..0c83ddc029 100644 --- a/.github/workflows/jazzy-coverage-build.yml +++ b/.github/workflows/jazzy-coverage-build.yml @@ -1,31 +1,21 @@ name: Coverage Build - Jazzy on: workflow_dispatch: - # TODO(anyone) activate when branched for Jazzy - # push: - # branches: - # - master - # paths: - # - '**.hpp' - # - '**.h' - # - '**.cpp' - # - '.github/workflows/jazzy-coverage-build.yml' - # - '**/package.xml' - # - '**/CMakeLists.txt' - # - 'ros2_control.jazzy.repos' - # - 'codecov.yml' - # pull_request: - # branches: - # - master - # paths: - # - '**.hpp' - # - '**.h' - # - '**.cpp' - # - '.github/workflows/jazzy-coverage-build.yml' - # - '**/package.xml' - # - '**/CMakeLists.txt' - # - 'ros2_control.jazzy.repos' - # - 'codecov.yml' + pull_request: &event + branches: + - jazzy + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-coverage-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.jazzy.repos' + - 'codecov.yml' + push: *event jobs: coverage_jazzy: diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index 0f695ed8bc..002b8723ff 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -1,29 +1,30 @@ -name: Debian Jazzy Source Build +name: Jazzy - Debian Semi-Binary Build on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/jazzy-debian-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.jazzy.repos' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - debian_source_build: + debian_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master strategy: fail-fast: false @@ -32,5 +33,5 @@ jobs: with: ros_distro: ${{ matrix.ROS_DISTRO }} upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy skip_packages: rqt_controller_manager diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml index aab5ba52ac..5f94855963 100644 --- a/.github/workflows/jazzy-pre-commit.yml +++ b/.github/workflows/jazzy-pre-commit.yml @@ -4,7 +4,10 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy + push: + branches: + - jazzy concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/jazzy-pre-release.yml b/.github/workflows/jazzy-pre-release.yml new file mode 100644 index 0000000000..18a3a0a78b --- /dev/null +++ b/.github/workflows/jazzy-pre-release.yml @@ -0,0 +1,28 @@ +name: Jazzy - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - jazzy + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: jazzy + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} + prerelease_exclude_pkg: ackermann_steering_controller admittance_controller bicycle_steering_controller chained_filter_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster forward_command_controller gpio_controllers gps_sensor_broadcaster imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller motion_primitives_controllers omni_wheel_drive_controller parallel_gripper_controller pid_controller pose_broadcaster position_controllers range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes rqt_joint_trajectory_controller steering_controllers_library tricycle_controller tricycle_steering_controller velocity_controllers gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests diff --git a/.github/workflows/jazzy-rhel-binary-build.yml b/.github/workflows/jazzy-rhel-binary-build.yml index a6404d2dbd..887e1c7bf2 100644 --- a/.github/workflows/jazzy-rhel-binary-build.yml +++ b/.github/workflows/jazzy-rhel-binary-build.yml @@ -1,26 +1,27 @@ -name: RHEL Jazzy Semi-Binary Build +name: Jazzy - RHEL Semi-Binary Build on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/jazzy-rhel-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.jazzy.repos' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: rhel_semi_binary_build: @@ -32,5 +33,5 @@ jobs: with: ros_distro: ${{ matrix.ROS_DISTRO }} upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy skip_packages: rqt_controller_manager diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index d5e3a96835..805061ea47 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -4,59 +4,44 @@ name: Jazzy Semi-Binary Build on: workflow_dispatch: - pull_request: + pull_request: &event branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/jazzy-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.jazzy.repos' - push: - branches: - - master + - jazzy paths: - '**.hpp' - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/jazzy-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.jazzy.repos' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: semi-binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [jazzy] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: master + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_control.jazzy.repos + ref_for_scheduled_build: jazzy 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 + ref_for_scheduled_build: jazzy additional_debs: clang c_compiler: clang cxx_compiler: clang++ diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml index 645941ee95..825a5bc53b 100644 --- a/.github/workflows/jazzy-semi-binary-downstream-build.yml +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -6,7 +6,7 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -28,24 +28,10 @@ jobs: with: ros_distro: jazzy ros_repo: testing - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy upstream_workspace: ros2_control.jazzy.repos # we don't test target_workspace, 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 target_workspace, we just build it - not_test_build: true - # we don't test the downstream packages, which are outside of our organization - downstream_workspace: | # build also the ros_controls packages - ros_controls.jazzy.repos - downstream.jazzy.repos - not_test_downstream: true diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml index 65066a4bf2..a63511aa9f 100644 --- a/.github/workflows/jazzy-source-build.yml +++ b/.github/workflows/jazzy-source-build.yml @@ -3,7 +3,7 @@ on: workflow_dispatch: push: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -13,15 +13,19 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.jazzy.repos' + pull_request: + branches: + - jazzy + paths: + - .github/workflows/jazzy-source-build.yml schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 3 * * MON-FRI' jobs: source: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: jazzy - ref: master - ros2_repo_branch: master + ref: jazzy container: ubuntu:24.04 diff --git a/.github/workflows/kilted-abi-compatibility.yml b/.github/workflows/kilted-abi-compatibility.yml new file mode 100644 index 0000000000..ac5b1ca50b --- /dev/null +++ b/.github/workflows/kilted-abi-compatibility.yml @@ -0,0 +1,27 @@ +name: Kilted - ABI Compatibility Check +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control-not-released.kilted.repos' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + abi_check: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: kilted diff --git a/.github/workflows/kilted-binary-build.yml b/.github/workflows/kilted-binary-build.yml new file mode 100644 index 0000000000..a6035d6e1c --- /dev/null +++ b/.github/workflows/kilted-binary-build.yml @@ -0,0 +1,42 @@ +name: Kilted Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control-not-released.kilted.repos' + push: *event + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + 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 + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [kilted] + ROS_REPO: [main, testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control-not-released.${{ matrix.ROS_DISTRO }}.repos + ref_for_scheduled_build: kilted diff --git a/.github/workflows/kilted-check-docs.yml b/.github/workflows/kilted-check-docs.yml new file mode 100644 index 0000000000..7998258907 --- /dev/null +++ b/.github/workflows/kilted-check-docs.yml @@ -0,0 +1,29 @@ +name: Kilted Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.rst' + - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' + - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/kilted-check-docs.yml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@kilted + with: + ROS2_CONTROL_PR: ${{ github.ref }} diff --git a/.github/workflows/kilted-coverage-build.yml b/.github/workflows/kilted-coverage-build.yml new file mode 100644 index 0000000000..5146b14c81 --- /dev/null +++ b/.github/workflows/kilted-coverage-build.yml @@ -0,0 +1,25 @@ +name: Coverage Build - Kilted +on: + workflow_dispatch: + pull_request: &event + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-coverage-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.kilted.repos' + - 'codecov.yml' + push: *event + +jobs: + coverage_kilted: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: kilted diff --git a/.github/workflows/kilted-debian-build.yml b/.github/workflows/kilted-debian-build.yml new file mode 100644 index 0000000000..e8faa042f7 --- /dev/null +++ b/.github/workflows/kilted-debian-build.yml @@ -0,0 +1,37 @@ +name: Kilted - Debian Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.kilted.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + debian_semi_binary_build: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [kilted] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos + ref_for_scheduled_build: kilted + skip_packages: rqt_controller_manager diff --git a/.github/workflows/kilted-pre-commit.yml b/.github/workflows/kilted-pre-commit.yml new file mode 100644 index 0000000000..fc2d6d0858 --- /dev/null +++ b/.github/workflows/kilted-pre-commit.yml @@ -0,0 +1,20 @@ +name: Pre-Commit - Kilted + +on: + workflow_dispatch: + pull_request: + branches: + - kilted + push: + branches: + - kilted + +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 + with: + ros_distro: kilted diff --git a/.github/workflows/kilted-pre-release.yml b/.github/workflows/kilted-pre-release.yml new file mode 100644 index 0000000000..c2c6b4042b --- /dev/null +++ b/.github/workflows/kilted-pre-release.yml @@ -0,0 +1,27 @@ +name: Kilted - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - kilted + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: kilted + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} diff --git a/.github/workflows/kilted-rhel-binary-build.yml b/.github/workflows/kilted-rhel-binary-build.yml new file mode 100644 index 0000000000..2f227d1233 --- /dev/null +++ b/.github/workflows/kilted-rhel-binary-build.yml @@ -0,0 +1,37 @@ +name: Kilted - RHEL Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-rhel-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.kilted.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + 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 + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [kilted] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos + ref_for_scheduled_build: kilted + skip_packages: rqt_controller_manager diff --git a/.github/workflows/kilted-semi-binary-build.yml b/.github/workflows/kilted-semi-binary-build.yml new file mode 100644 index 0000000000..45a96814c9 --- /dev/null +++ b/.github/workflows/kilted-semi-binary-build.yml @@ -0,0 +1,48 @@ +name: Kilted Semi-Binary Build +# author: Denis Štogl +# description: 'Build & test all ros2_control dependencies from source.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.kilted.repos' + push: *event + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + semi-binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: kilted + ros_repo: testing + upstream_workspace: ros2_control.kilted.repos + ref_for_scheduled_build: kilted + semi-binary-clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: kilted + ros_repo: testing + upstream_workspace: ros2_control.kilted.repos + ref_for_scheduled_build: kilted + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/kilted-semi-binary-downstream-build.yml b/.github/workflows/kilted-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..4f45ead4c4 --- /dev/null +++ b/.github/workflows/kilted-semi-binary-downstream-build.yml @@ -0,0 +1,37 @@ +name: Kilted Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.kilted.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: kilted + ros_repo: testing + ref_for_scheduled_build: kilted + upstream_workspace: ros2_control.kilted.repos + # we don't test target_workspace, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.kilted.repos + not_test_downstream: false diff --git a/.github/workflows/kilted-source-build.yml b/.github/workflows/kilted-source-build.yml new file mode 100644 index 0000000000..53bf277d12 --- /dev/null +++ b/.github/workflows/kilted-source-build.yml @@ -0,0 +1,32 @@ +name: Kilted Source Build +on: + workflow_dispatch: + push: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.kilted.repos' + pull_request: + branches: + - kilted + paths: + - .github/workflows/kilted-source-build.yml + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * MON-FRI' + +jobs: + source: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master + with: + ros_distro: kilted + ref: kilted + container: ubuntu:24.04 diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml deleted file mode 100644 index 182df6e22b..0000000000 --- a/.github/workflows/prerelease-check.yml +++ /dev/null @@ -1,37 +0,0 @@ -name: Pre-Release Check - -on: - workflow_dispatch: - inputs: - ros_distro: - description: 'Chose ROS distribution' - required: true - default: 'rolling' - type: choice - options: - - humble - - iron - - rolling - branch: - description: 'Chose branch for distro' - required: true - default: 'master' - type: choice - options: - - humble - - iron - - master - -jobs: - pre_release: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - ref: ${{ github.event.inputs.branch }} - - name: industrial_ci - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: ${{ github.event.inputs.ros_distro }} - PRERELEASE: true - BASEDIR: ${{ github.workspace }}/.work diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 02a38494b5..fe865b8e88 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -9,18 +9,24 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-abi-compatibility.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.rolling.repos' + - 'ros2_control-not-released.kilted.repos' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: abi_check: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 0859169b15..05c3e09b97 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -4,19 +4,7 @@ name: Rolling Binary Build on: workflow_dispatch: - pull_request: - branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/rolling-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control-not-released.rolling.repos' - push: + pull_request: &event branches: - master paths: @@ -24,18 +12,21 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.rolling.repos' + - 'ros2_control-not-released.kilted.repos' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: binary: diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 085ee7f9d1..bac1a6dd86 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -8,7 +8,13 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory - '.github/workflows/rolling-check-docs.yml' concurrency: diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 35957bbc0e..a5bec4e952 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -4,19 +4,7 @@ name: Check Rolling Compatibility on Humble on: workflow_dispatch: - pull_request: - branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/rolling-compatibility-humble-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.rolling.repos' - push: + pull_request: &event branches: - master paths: @@ -24,26 +12,23 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-compatibility-humble-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + 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 - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: humble + ros_repo: testing upstream_workspace: ros2_control.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index bddbd14878..01d8ba423f 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -4,19 +4,7 @@ name: Check Rolling Compatibility on Jazzy on: workflow_dispatch: - pull_request: - branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.rolling.repos' - push: + pull_request: &event branches: - master paths: @@ -24,26 +12,23 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + 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 - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [jazzy] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: jazzy + ros_repo: testing upstream_workspace: ros2_control.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-kilted-binary-build.yml b/.github/workflows/rolling-compatibility-kilted-binary-build.yml new file mode 100644 index 0000000000..45097e424e --- /dev/null +++ b/.github/workflows/rolling-compatibility-kilted-binary-build.yml @@ -0,0 +1,34 @@ +name: Check Rolling Compatibility on Kilted +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Kilted distro.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-kilted-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: *event + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + build-on-kilted: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: kilted + ros_repo: testing + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index ad4e467606..bef06d3622 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -1,20 +1,7 @@ name: Coverage Build - Rolling on: workflow_dispatch: - push: - branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/rolling-coverage-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.rolling.repos' - - 'codecov.yml' - pull_request: + pull_request: &event branches: - master paths: @@ -22,16 +9,18 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-coverage-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - 'codecov.yml' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: coverage_rolling: diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index a54e7ad0d1..01d59bbc28 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Rolling Source Build +name: Rolling - Debian Semi-Binary Build on: workflow_dispatch: pull_request: @@ -9,22 +9,24 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-debian-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' + - 'ros2_control.kilted.repos' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - debian_source_build: + debian_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master strategy: fail-fast: false diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 792278d6d2..54d7e90adf 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -5,6 +5,9 @@ on: pull_request: branches: - master + push: + branches: + - master concurrency: group: ${{ github.workflow }}-${{ github.ref }} @@ -13,5 +16,9 @@ concurrency: jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-pre-release.yml b/.github/workflows/rolling-pre-release.yml new file mode 100644 index 0000000000..71ac765177 --- /dev/null +++ b/.github/workflows/rolling-pre-release.yml @@ -0,0 +1,33 @@ +name: Rolling - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - master + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} + prerelease_exclude_pkg: ackermann_steering_controller admittance_controller bicycle_steering_controller chained_filter_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster forward_command_controller gpio_controllers gps_sensor_broadcaster imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller motion_primitives_controllers omni_wheel_drive_controller parallel_gripper_controller pid_controller pose_broadcaster position_controllers range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes rqt_joint_trajectory_controller steering_controllers_library tricycle_controller tricycle_steering_controller velocity_controllers gz_ros2_control gz_ros2_control_demos gz_ros2_control_tests diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 95d6759da6..3d39bd3262 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,4 +1,4 @@ -name: RHEL Rolling Semi-Binary Build +name: Rolling - RHEL Semi-Binary Build on: workflow_dispatch: pull_request: @@ -9,18 +9,20 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-rhel-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - - 'ros2_control.jazzy.repos' + - 'ros2_control.rolling.repos' + - 'ros2_control.kilted.repos' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: rhel_semi_binary_build: diff --git a/.github/workflows/rolling-semi-binary-build-win.yml b/.github/workflows/rolling-semi-binary-build-win.yml new file mode 100644 index 0000000000..efc66c2fac --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-win.yml @@ -0,0 +1,33 @@ +name: Rolling Windows Semi-Binary Build +# author: Christoph Fröhlich +# description: 'Build & test all dependencies from semi-binary packages.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build-win.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + push: *event + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + binary-windows: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-win-build.yml@master + with: + ros_distro: rolling + pixi_dependencies: typeguard jinja2 boost compilers + ninja_packages: rsl + target_cmake_args: -DBUILD_TESTING=OFF diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 6431b9f4fa..b8256d39f5 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -4,19 +4,7 @@ name: Rolling Semi-Binary Build on: workflow_dispatch: - pull_request: - branches: - - master - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/rolling-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.rolling.repos' - push: + pull_request: &event branches: - master paths: @@ -24,18 +12,21 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' + - 'ros2_control.kilted.repos' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: semi-binary: @@ -44,19 +35,21 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [rolling] - ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing 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 + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - # job for building only, no tests -> one distro is enough - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: testing - upstream_workspace: ros2_control.rolling.repos + upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master additional_debs: clang c_compiler: clang diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml index 67d22def26..13735752a9 100644 --- a/.github/workflows/rolling-semi-binary-downstream-build.yml +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -17,6 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.rolling.repos' + - 'ros_controls.kilted.repos' concurrency: group: ${{ github.workflow }}-${{ github.ref }} @@ -39,21 +40,3 @@ jobs: # 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 target_workspace, we just build it - not_test_build: true - # we don't test the downstream packages, which are outside of our organization - downstream_workspace: | # build also the ros_controls packages - ros_controls.${{ matrix.ROS_DISTRO }}.repos - downstream.${{ matrix.ROS_DISTRO }}.repos - not_test_downstream: true diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 9bbf09cda4..4c07772dcd 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -9,19 +9,29 @@ on: - '**.h' - '**.cpp' - '**.py' + - '**.yaml' - '.github/workflows/rolling-source-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' + - 'ros2_control.kilted.repos' + pull_request: + branches: + - master + paths: + - .github/workflows/rolling-source-build.yml schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 3 * * MON-FRI' jobs: source: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} ref: master - ros2_repo_branch: master container: ubuntu:24.04 diff --git a/.github/workflows/stale.yml b/.github/workflows/stale.yml index f30de29b75..75b6b01885 100644 --- a/.github/workflows/stale.yml +++ b/.github/workflows/stale.yml @@ -12,7 +12,7 @@ jobs: issues: write pull-requests: write steps: - - uses: actions/stale@v9 + - uses: actions/stale@v10 with: stale-issue-label: 'stale' stale-pr-label: 'stale' diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ee0e31793a..78866ca277 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 + rev: v6.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.1 + rev: v3.21.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,20 +50,20 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 25.1.0 + rev: 25.9.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.2.0 + rev: 7.3.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v20.1.3 + rev: v21.1.2 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -109,7 +109,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.2 + rev: v2.0.0 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.33.0 + rev: 0.34.1 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/README.md b/README.md index 0842949073..5dbd61f951 100644 --- a/README.md +++ b/README.md @@ -13,11 +13,12 @@ If you are new to the project, please read the [contributing guide](https://cont ## Build status -ROS2 Distro | Branch | Build status | Documentation | Released packages +ROS2 Distro | Branch | Build status | Documentation | Package Build :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__ros2_control__ubuntu_noble_amd64/) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__ros2_control__ubuntu_noble_amd64/) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__ros2_control__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__ros2_control__ubuntu_jammy_amd64/) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__ros2_control__ubuntu_noble_amd64/) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__ros2_control__ubuntu_noble_amd64__binary/) +**Kilted** | [`kilted`](https://github.com/ros-controls/ros2_control/tree/kilted) | [![Kilted Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/kilted-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/kilted-binary-build.yml?branch=master)
[![Kilted Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/kilted-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/kilted-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__ros2_control__ubuntu_noble_amd64/) | [Documentation](https://control.ros.org/kilted/index.html)
[API Reference](https://control.ros.org/kilted/doc/api/index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__ros2_control__ubuntu_noble_amd64__binary/) +**Jazzy** | [`jazzy`](https://github.com/ros-controls/ros2_control/tree/jazzy) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__ros2_control__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__ros2_control__ubuntu_noble_amd64/) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__ros2_control__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__ros2_control__ubuntu_noble_amd64__binary/) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__ros2_control__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__ros2_control__ubuntu_jammy_amd64/) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__ros2_control__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__ros2_control__ubuntu_jammy_amd64__binary/) ## Docker images diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 9148df31b8..6c83dc6a91 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,53 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ +* Add magnetic_field_sensor semantic component (`#2627 `_) +* Fix `-Wreturn-local-addr` compiler warning (`#2628 `_) +* [Controllers] Set async thread properties via parameters (`#2613 `_) +* Contributors: Aarav Gupta, Christoph Fröhlich, Sai Kishor Kothakota + +5.7.0 (2025-10-03) +------------------ +* Cleanup deprecations for kilted release (`#2605 `_) +* Add `ControllerInterfaceParams` to initialize the Controllers (`#2390 `_) +* Let `get_ordered_interfaces` throw if input vector size does not fit (`#2528 `_) +* Update message dependencies for tests (`#2497 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ +* Fix missing include for std::find (`#2425 `_) +* Document order of interfaces (`#2394 `_) +* Contributors: Christoph Fröhlich, Guilhem Saurel + +5.4.0 (2025-07-21) +------------------ + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ +* Use target_link_libraries instead of ament_target_dependencies (`#2266 `_) +* Cleanup deprecations in `ros_control` (`#2258 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* Statically allocate string concatenations using FMT formatting (`#2205 `_) +* Contributors: mini-1235 + 4.29.0 (2025-05-04) ------------------- * Add common reusable helper methods header (`#2099 `_) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 069f968528..52105df453 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -9,6 +9,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface rclcpp_lifecycle realtime_tools + fmt ) find_package(ament_cmake REQUIRED) @@ -27,12 +28,17 @@ target_include_directories(controller_interface PUBLIC $ $ ) -ament_target_dependencies(controller_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(controller_interface PUBLIC + hardware_interface::hardware_interface + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + fmt::fmt) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) + find_package(std_msgs REQUIRED) ament_add_gmock(test_controller_interface test/test_controller_interface.cpp) target_link_libraries(test_controller_interface @@ -63,29 +69,35 @@ if(BUILD_TESTING) target_link_libraries(test_force_torque_sensor controller_interface hardware_interface::hardware_interface + ${geometry_msgs_TARGETS} ) ament_add_gmock(test_imu_sensor test/test_imu_sensor.cpp) target_link_libraries(test_imu_sensor controller_interface hardware_interface::hardware_interface + ${sensor_msgs_TARGETS} ) - ament_target_dependencies(test_imu_sensor - sensor_msgs + + ament_add_gmock(test_magnetic_field_sensor test/test_magnetic_field_sensor.cpp) + target_link_libraries(test_magnetic_field_sensor + controller_interface + hardware_interface::hardware_interface + ${sensor_msgs_TARGETS} ) ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp) target_link_libraries(test_pose_sensor controller_interface hardware_interface::hardware_interface + ${geometry_msgs_TARGETS} ) - ament_target_dependencies(test_pose_sensor - geometry_msgs - ) + ament_add_gmock(test_gps_sensor test/test_gps_sensor.cpp) target_link_libraries(test_gps_sensor controller_interface hardware_interface::hardware_interface + ${sensor_msgs_TARGETS} ) # Semantic component command interface tests @@ -96,15 +108,14 @@ if(BUILD_TESTING) target_link_libraries(test_semantic_component_command_interface controller_interface hardware_interface::hardware_interface + ${geometry_msgs_TARGETS} ) ament_add_gmock(test_led_rgb_device test/test_led_rgb_device.cpp) target_link_libraries(test_led_rgb_device controller_interface hardware_interface::hardware_interface - ) - ament_target_dependencies(test_led_rgb_device - std_msgs + ${std_msgs_TARGETS} ) endif() diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index a4d88e7153..94e36d4fc0 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -17,11 +17,13 @@ #include #include +#include #include #include #include "realtime_tools/async_function_handler.hpp" +#include "controller_interface/controller_interface_params.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/introspection.hpp" #include "hardware_interface/loaned_command_interface.hpp" @@ -151,10 +153,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy */ virtual void release_interfaces(); + [[deprecated( + "Use the init(const controller_interface::ControllerInterfaceParams & params) method instead. " + "This method will be removed in the future ROS 2 releases.")]] return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options); + return_type init(const controller_interface::ControllerInterfaceParams & params); + /// Custom configure method to read additional parameters for controller-nodes /* * Override default implementation for configure of LifecycleNode to get parameters. @@ -201,6 +208,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy const std::string & get_robot_description() const; + /** + * Get the unordered map of joint limits that are defined in the robot description. + */ + const std::unordered_map & get_hard_joint_limits() const; + + /** + * Get the unordered map of soft joint limits that are defined in the robot description. + */ + const std::unordered_map & get_soft_joint_limits() + const; + /** * Method used by the controller_manager for base NodeOptions to instantiate the Lifecycle node * of the controller upon loading the controller. @@ -328,7 +346,25 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy void enable_introspection(bool enable); protected: + /** Loaned command interfaces. + * \note The order of these interfaces is determined by the return value of + * \ref command_interface_configuration(): + * If interface_configuration_type::INDIVIDUAL is specified, the order matches that of the + * returned vector. + * If interface_configuration_type::ALL is specified, the order is determined by the internal + * memory of the resource_manager and may not be deterministic. To obtain a consistent order, use + * \ref get_ordered_interfaces() from \ref helpers.hpp. + */ std::vector command_interfaces_; + /** Loaned state interfaces. + * \note The order of these interfaces is determined by the return value of + * \ref state_interface_configuration(): + * If interface_configuration_type::INDIVIDUAL is specified, the order matches that of the + * returned vector. + * If interface_configuration_type::ALL is specified, the order is determined by the internal + * memory of the resource_manager and may not be deterministic. To obtain a consistent order, use + * \ref get_ordered_interfaces() from \ref helpers.hpp. + */ std::vector state_interfaces_; private: @@ -340,9 +376,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr node_; std::unique_ptr> async_handler_; - unsigned int update_rate_ = 0; bool is_async_ = false; - std::string urdf_ = ""; + controller_interface::ControllerInterfaceParams ctrl_itf_params_; std::atomic_bool skip_async_triggers_ = false; ControllerUpdateStats trigger_stats_; diff --git a/controller_interface/include/controller_interface/controller_interface_params.hpp b/controller_interface/include/controller_interface/controller_interface_params.hpp new file mode 100644 index 0000000000..eddd29117e --- /dev/null +++ b/controller_interface/include/controller_interface/controller_interface_params.hpp @@ -0,0 +1,60 @@ +// Copyright 2025 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 CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_PARAMS_HPP_ +#define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_PARAMS_HPP_ + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/node_options.hpp" + +/** + * @brief Parameters for the Controller Interface + * This struct holds the parameters required to initialize a controller interface. + * + * @var controller_name Name of the controller. + * @var robot_description The URDF or SDF description of the robot. + * @var cm_update_rate The update rate of the controller manager in Hz. + * @var node_namespace The namespace for the controller node. + * @var node_options Options for the controller node. + * @var joint_limits A map of joint names to their limits. + * @var soft_joint_limits A map of joint names to their soft limits. + * + * This struct is used to pass parameters to the controller interface during initialization. + * It allows for easy configuration of the controller's behavior and interaction with the robot's + * joints. + */ +namespace controller_interface +{ + +struct ControllerInterfaceParams +{ + ControllerInterfaceParams() = default; + + std::string controller_name = ""; + std::string robot_description = ""; + unsigned int update_rate = 0; + + std::string node_namespace = ""; + rclcpp::NodeOptions node_options = rclcpp::NodeOptions(); + + std::unordered_map hard_joint_limits = {}; + std::unordered_map soft_joint_limits = {}; +}; + +} // namespace controller_interface + +#endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_PARAMS_HPP_ diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index d9ca03eb5b..ffc1fc9b4f 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -15,7 +15,10 @@ #ifndef CONTROLLER_INTERFACE__HELPERS_HPP_ #define CONTROLLER_INTERFACE__HELPERS_HPP_ +#include #include +#include +#include #include #include @@ -36,7 +39,10 @@ namespace controller_interface * If joint names are used for ordering, \p interface_type specifies valid interface. * If full interface names are used for ordering, \p interface_type should be empty string (""). * \param[in] interface_type used for ordering interfaces with respect to joint names. - * \param[out] ordered_interfaces vector with ordered interfaces. + * \param[out] ordered_interfaces vector with ordered interfaces. Has to have the same capacity as + * \p ordered_names size. Throws otherwise. + * \throws std::range_error if the capacity of ordered_interfaces is less than the size of + * ordered_names. * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. */ template @@ -44,7 +50,13 @@ bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & ordered_names, const std::string & interface_type, std::vector> & ordered_interfaces) { - ordered_interfaces.reserve(ordered_names.size()); + if (ordered_interfaces.capacity() < ordered_names.size()) + { + throw std::range_error( + "Capacity of ordered_interfaces (" + std::to_string(ordered_interfaces.capacity()) + + ") has to be equal or higher as size of ordered_names (" + + std::to_string(ordered_names.size()) + ") for realtime reasons."); + } for (const auto & name : ordered_names) { for (auto & interface : unordered_interfaces) @@ -81,15 +93,6 @@ inline bool interface_list_contains_interface_type( interface_type_list.end(); } -template -[[deprecated( - "Use ros2_control::add_item method instead. This method will be removed by the ROS 2 Kilted " - "Kaiju release.")]] void -add_element_to_list(std::vector & list, const T & element) -{ - ros2_control::add_item(list, element); -} - } // namespace controller_interface #endif // CONTROLLER_INTERFACE__HELPERS_HPP_ diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 6d70e8be53..38c788db05 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -52,6 +52,7 @@ class GPSSensor : public SemanticComponentInterface interface_names_.emplace_back(name + "/" + "latitude_covariance"); interface_names_.emplace_back(name + "/" + "longitude_covariance"); interface_names_.emplace_back(name + "/" + "altitude_covariance"); + state_interfaces_.reserve(state_interfaces_.capacity() + 3); } } diff --git a/controller_interface/include/semantic_components/magnetic_field_sensor.hpp b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp new file mode 100644 index 0000000000..7ce424cc3f --- /dev/null +++ b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp @@ -0,0 +1,73 @@ +// Copyright 2025 Aarav Gupta +// +// 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 SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_ +#define SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_ + +#include + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" + +namespace semantic_components +{ +class MagneticFieldSensor : public SemanticComponentInterface +{ +public: + explicit MagneticFieldSensor(const std::string & name) + : SemanticComponentInterface( + name, {name + "/" + "magnetic_field.x", name + "/" + "magnetic_field.y", + name + "/" + "magnetic_field.z"}) + { + } + + /// Returns values as sensor_msgs::msg::MagneticField + /** + * \return MagneticField message from values + */ + bool get_values_as_message(sensor_msgs::msg::MagneticField & message) + { + update_data_from_interfaces(); + message.magnetic_field.x = data_[0]; + message.magnetic_field.y = data_[1]; + message.magnetic_field.z = data_[2]; + return true; + } + +private: + /** + * @brief Update the data array from the state interfaces. + * @note This method is thread-safe and non-blocking. + * @note This method might return stale data if the data is not updated. This is to ensure that + * the data from the sensor is not discontinuous. + */ + void update_data_from_interfaces() + { + for (auto i = 0u; i < data_.size(); ++i) + { + const auto data = state_interfaces_[i].get().get_optional(); + if (data.has_value()) + { + data_[i] = data.value(); + } + } + } + + // Array to store the data of the magnetic field sensor + std::array data_{{0.0, 0.0, 0.0}}; +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_ diff --git a/controller_interface/include/semantic_components/semantic_component_interface.hpp b/controller_interface/include/semantic_components/semantic_component_interface.hpp index 76a34aaddd..ac652aa9fb 100644 --- a/controller_interface/include/semantic_components/semantic_component_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_interface.hpp @@ -104,6 +104,10 @@ class SemanticComponentInterface */ bool get_values_as_message(MessageReturnType & /* message */) { return false; } + // delete copy constructor, because + // copy will change capacity of member variables + SemanticComponentInterface(const SemanticComponentInterface &) = delete; + protected: std::string name_; std::vector interface_names_; diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 028f323c9e..2b0683dce0 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.29.0 + 6.0.1 Base classes for controllers and syntax cookies for supporting common sensor types in controllers and broadcasters Bence Magyar Denis Štogl @@ -17,7 +17,7 @@ rclcpp_lifecycle realtime_tools ros2_control_cmake - sensor_msgs + fmt hardware_interface realtime_tools @@ -28,6 +28,7 @@ ament_cmake_gmock geometry_msgs sensor_msgs + std_msgs ament_cmake diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 703e84b474..d13ec19129 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -14,6 +14,8 @@ #include "controller_interface/chainable_controller_interface.hpp" +#include + #include #include "controller_interface/helpers.hpp" @@ -62,12 +64,12 @@ ChainableControllerInterface::export_state_interfaces() { if (interface.get_prefix_name().find(get_node()->get_name()) != 0) { - std::string error_msg = - "The prefix of the interface '" + interface.get_prefix_name() + - "' should begin with the controller's name '" + get_node()->get_name() + - "'. This is mandatory for state interfaces. No state interface will be exported. Please " - "correct and recompile the controller with name '" + - get_node()->get_name() + "' and try again."; + const std::string error_msg = fmt::format( + FMT_COMPILE( + "The prefix of the interface '{}' should begin with the controller's name '{}'. " + "This is mandatory for state interfaces. No state interface will be exported. " + "Please correct and recompile the controller with name '{}' and try again."), + interface.get_prefix_name(), get_node()->get_name(), get_node()->get_name()); throw std::runtime_error(error_msg); } auto state_interface = std::make_shared(interface); @@ -78,13 +80,13 @@ ChainableControllerInterface::export_state_interfaces() // inform cm by throwing exception if (!succ) { - std::string error_msg = - "Could not insert StateInterface<" + interface_name + - "> into exported_state_interfaces_ map. Check if you export duplicates. The " - "map returned iterator with interface_name<" + - it->second->get_name() + - ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " - "interface names are unique."; + std::string error_msg = fmt::format( + FMT_COMPILE( + "Could not insert StateInterface<{}> into exported_state_interfaces_ map. " + "Check if you export duplicates. The map returned iterator with interface_name<{}>. " + "If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."), + interface_name, it->second->get_name()); exported_state_interfaces_.clear(); exported_state_interface_names_.clear(); state_interfaces_ptrs_vec.clear(); @@ -98,14 +100,13 @@ ChainableControllerInterface::export_state_interfaces() if (exported_state_interfaces_.size() != state_interfaces.size()) { - std::string error_msg = - "The internal storage for state interface ptrs 'exported_state_interfaces_' variable has " - "size '" + - std::to_string(exported_state_interfaces_.size()) + - "', but it is expected to have the size '" + std::to_string(state_interfaces.size()) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; + std::string error_msg = fmt::format( + FMT_COMPILE( + "The internal storage for state interface ptrs 'exported_state_interfaces_' variable has " + "size '{}', but it is expected to have the size '{}' equal to the number of exported " + "reference interfaces. Please correct and recompile the controller with name '{}' and try " + "again."), + exported_state_interfaces_.size(), state_interfaces.size(), get_node()->get_name()); throw std::runtime_error(error_msg); } @@ -128,13 +129,12 @@ ChainableControllerInterface::export_reference_interfaces() // check if the "reference_interfaces_" variable is resized to number of interfaces if (reference_interfaces_.size() != reference_interfaces.size()) { - std::string error_msg = - "The internal storage for reference values 'reference_interfaces_' variable has size '" + - std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + - std::to_string(reference_interfaces.size()) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; + std::string error_msg = fmt::format( + FMT_COMPILE( + "The internal storage for reference values 'reference_interfaces_' variable has size '{}', " + "but it is expected to have the size '{}' equal to the number of exported reference " + "interfaces. Please correct and recompile the controller with name '{}' and try again."), + reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); throw std::runtime_error(error_msg); } // END @@ -145,12 +145,12 @@ ChainableControllerInterface::export_reference_interfaces() { if (interface.get_prefix_name().find(get_node()->get_name()) != 0) { - std::string error_msg = "The prefix of the interface '" + interface.get_prefix_name() + - "' should begin with the controller's name '" + - get_node()->get_name() + - "'. This is mandatory for reference interfaces. Please correct and " - "recompile the controller with name '" + - get_node()->get_name() + "' and try again."; + std::string error_msg = fmt::format( + FMT_COMPILE( + "The prefix of the interface '{}' should begin with the controller's name '{}'. " + "This is mandatory for reference interfaces. Please correct and recompile the " + "controller with name '{}' and try again."), + interface.get_prefix_name(), get_node()->get_name(), get_node()->get_name()); throw std::runtime_error(error_msg); } @@ -164,13 +164,13 @@ ChainableControllerInterface::export_reference_interfaces() // inform cm by throwing exception if (!succ) { - std::string error_msg = - "Could not insert Reference interface<" + interface_name + - "> into reference_interfaces_ map. Check if you export duplicates. The " - "map returned iterator with interface_name<" + - it->second->get_name() + - ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " - "interface names are unique."; + std::string error_msg = fmt::format( + FMT_COMPILE( + "Could not insert Reference interface<{}> into reference_interfaces_ map. " + "Check if you export duplicates. The map returned iterator with interface_name<{}>. " + "If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."), + interface_name, it->second->get_name()); reference_interfaces_.clear(); exported_reference_interface_names_.clear(); reference_interfaces_ptrs_vec.clear(); @@ -183,14 +183,13 @@ ChainableControllerInterface::export_reference_interfaces() if (exported_reference_interfaces_.size() != ref_interface_size) { - std::string error_msg = - "The internal storage for exported reference ptrs 'exported_reference_interfaces_' variable " - "has size '" + - std::to_string(exported_reference_interfaces_.size()) + - "', but it is expected to have the size '" + std::to_string(ref_interface_size) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; + std::string error_msg = fmt::format( + FMT_COMPILE( + "The internal storage for exported reference ptrs 'exported_reference_interfaces_' " + "variable has size '{}', but it is expected to have the size '{}' equal to the number of " + "exported reference interfaces. Please correct and recompile the controller with name '{}' " + "and try again."), + exported_reference_interfaces_.size(), ref_interface_size, get_node()->get_name()); throw std::runtime_error(error_msg); } diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 532544ab30..50bef315f1 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -43,19 +43,32 @@ return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { - urdf_ = urdf; - update_rate_ = cm_update_rate; + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = urdf; + params.update_rate = cm_update_rate; + params.node_namespace = node_namespace; + params.node_options = node_options; + + return init(params); +} + +return_type ControllerInterfaceBase::init( + const controller_interface::ControllerInterfaceParams & params) +{ + ctrl_itf_params_ = params; node_ = std::make_shared( - controller_name, node_namespace, node_options, + ctrl_itf_params_.controller_name, ctrl_itf_params_.node_namespace, + ctrl_itf_params_.node_options, false); // disable LifecycleNode service interfaces try { // no rclcpp::ParameterValue unsigned int specialization - auto_declare("update_rate", static_cast(update_rate_)); + auto_declare("update_rate", static_cast(ctrl_itf_params_.update_rate)); auto_declare("is_async", false); - auto_declare("thread_priority", 50); + auto_declare("thread_priority", -100); } catch (const std::exception & e) { @@ -149,32 +162,51 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!"); return get_lifecycle_state(); } - if (update_rate_ != 0u && update_rate > update_rate_) + if (ctrl_itf_params_.update_rate != 0u && update_rate > ctrl_itf_params_.update_rate) { RCLCPP_WARN( get_node()->get_logger(), "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the " "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.", - update_rate, update_rate_); + update_rate, ctrl_itf_params_.update_rate); } else { - update_rate_ = static_cast(update_rate); + ctrl_itf_params_.update_rate = static_cast(update_rate); } is_async_ = get_node()->get_parameter("is_async").as_bool(); } if (is_async_) { - const int thread_priority = + realtime_tools::AsyncFunctionHandlerParams async_params; + async_params.thread_priority = 50; // default value + const int thread_priority_param = static_cast(get_node()->get_parameter("thread_priority").as_int()); + if (thread_priority_param >= 0 && thread_priority_param <= 99) + { + async_params.thread_priority = thread_priority_param; + RCLCPP_WARN( + get_node()->get_logger(), + "The parsed 'thread_priority' parameter will be deprecated and not be functional from " + "ROS 2 Lyrical Luth release. Please use the 'async_parameters.thread_priority' parameter " + "instead."); + } + async_params.initialize(node_, "async_parameters."); + if (async_params.scheduling_policy == realtime_tools::AsyncSchedulingPolicy::DETACHED) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The controllers are not supported to run asynchronously in detached mode!"); + return get_node()->get_current_state(); + } RCLCPP_INFO( get_node()->get_logger(), "Starting async handler with scheduler priority: %d", - thread_priority); + async_params.thread_priority); async_handler_ = std::make_unique>(); async_handler_->init( std::bind( &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2), - thread_priority); + async_params); async_handler_->start_thread(); } REGISTER_ROS2_CONTROL_INTROSPECTION("total_triggers", &trigger_stats_.total_triggers); @@ -273,11 +305,29 @@ std::shared_ptr ControllerInterfaceBase:: return node_; } -unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; } +unsigned int ControllerInterfaceBase::get_update_rate() const +{ + return ctrl_itf_params_.update_rate; +} bool ControllerInterfaceBase::is_async() const { return is_async_; } -const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } +const std::string & ControllerInterfaceBase::get_robot_description() const +{ + return ctrl_itf_params_.robot_description; +} + +const std::unordered_map & +ControllerInterfaceBase::get_hard_joint_limits() const +{ + return ctrl_itf_params_.hard_joint_limits; +} + +const std::unordered_map & +ControllerInterfaceBase::get_soft_joint_limits() const +{ + return ctrl_itf_params_.soft_joint_limits; +} void ControllerInterfaceBase::wait_for_trigger_update_to_finish() { diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index b0b82f05f0..3f0b564c7d 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -26,10 +26,13 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) TestableChainableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_TRUE(controller.is_chainable()); @@ -41,10 +44,13 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) TestableChainableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto exported_state_interfaces = controller.export_state_interfaces(); @@ -68,10 +74,13 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) TestableChainableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -95,10 +104,13 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) TestableChainableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); @@ -121,10 +133,13 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TestableChainableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -176,10 +191,13 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) TestableChainableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_FALSE(controller.set_chained_mode(false)); diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 286e132003..11b782eb65 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -44,10 +44,13 @@ TEST(TestableControllerInterface, init) ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error); // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 10; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); ASSERT_NO_THROW(const_controller.get_node()); ASSERT_NO_THROW(controller.get_lifecycle_state()); @@ -76,9 +79,13 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) node_options_arguments.push_back("-p"); node_options_arguments.push_back("update_rate:=-100"); node_options = node_options.arguments(node_options_arguments); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 1000; + params.node_namespace = ""; + params.node_options = node_options; + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); // update_rate is set to controller_manager's rate ASSERT_EQ(controller.get_update_rate(), 1000u); @@ -104,9 +111,21 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) node_options_arguments.push_back("-p"); node_options_arguments.push_back("update_rate:=2812"); node_options = node_options.arguments(node_options_arguments); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 5000; // set a different update rate than the one in the node options + params.node_namespace = ""; + params.node_options = node_options; + joint_limits::JointLimits joint_limits; + joint_limits.has_velocity_limits = true; + joint_limits.max_velocity = 1.0; + params.hard_joint_limits["joint1"] = joint_limits; + joint_limits::SoftJointLimits soft_joint_limits; + soft_joint_limits.min_position = -1.0; + soft_joint_limits.max_position = 1.0; + params.soft_joint_limits["joint1"] = soft_joint_limits; + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); // initialize executor to be able to get parameter update auto executor = @@ -122,6 +141,21 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) // update_rate is set to controller_manager's rate ASSERT_EQ(controller.get_update_rate(), 5000u); + const auto hard_limits = controller.get_hard_joint_limits(); + const auto soft_limits = controller.get_soft_joint_limits(); + ASSERT_THAT(hard_limits, testing::SizeIs(1)); + ASSERT_TRUE(hard_limits.find("joint1") != hard_limits.end()); + ASSERT_THAT(hard_limits.at("joint1").max_velocity, joint_limits.max_velocity); + ASSERT_TRUE(hard_limits.at("joint1").has_velocity_limits); + ASSERT_FALSE(hard_limits.at("joint1").has_position_limits); + ASSERT_FALSE(hard_limits.at("joint1").has_acceleration_limits); + ASSERT_FALSE(hard_limits.at("joint1").has_deceleration_limits); + ASSERT_FALSE(hard_limits.at("joint1").has_jerk_limits); + ASSERT_FALSE(hard_limits.at("joint1").has_effort_limits); + ASSERT_THAT(soft_limits, testing::SizeIs(1)); + ASSERT_TRUE(soft_limits.find("joint1") != soft_limits.end()); + ASSERT_THAT(soft_limits.at("joint1").min_position, soft_joint_limits.min_position); + ASSERT_THAT(soft_limits.at("joint1").max_position, soft_joint_limits.max_position); // Even after configure is 0 controller.configure(); @@ -156,6 +190,23 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) controller.configure(); ASSERT_EQ(controller.get_update_rate(), 623u); + // Should stay same after multiple cleanups as it is set during initialization + const auto hard_limits_final = controller.get_hard_joint_limits(); + const auto soft_limits_final = controller.get_soft_joint_limits(); + ASSERT_THAT(hard_limits_final, testing::SizeIs(1)); + ASSERT_TRUE(hard_limits_final.find("joint1") != hard_limits_final.end()); + ASSERT_THAT(hard_limits_final.at("joint1").max_velocity, joint_limits.max_velocity); + ASSERT_TRUE(hard_limits_final.at("joint1").has_velocity_limits); + ASSERT_FALSE(hard_limits_final.at("joint1").has_position_limits); + ASSERT_FALSE(hard_limits_final.at("joint1").has_acceleration_limits); + ASSERT_FALSE(hard_limits_final.at("joint1").has_deceleration_limits); + ASSERT_FALSE(hard_limits_final.at("joint1").has_jerk_limits); + ASSERT_FALSE(hard_limits_final.at("joint1").has_effort_limits); + ASSERT_THAT(soft_limits_final, testing::SizeIs(1)); + ASSERT_TRUE(soft_limits_final.find("joint1") != soft_limits_final.end()); + ASSERT_THAT(soft_limits_final.at("joint1").min_position, soft_joint_limits.min_position); + ASSERT_THAT(soft_limits_final.at("joint1").max_position, soft_joint_limits.max_position); + executor->cancel(); controller.get_node()->shutdown(); rclcpp::shutdown(); @@ -170,10 +221,13 @@ TEST(TestableControllerInterfaceInitError, init_with_error) TestableControllerInterfaceInitError controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options), - controller_interface::return_type::ERROR); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 100; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::ERROR); ASSERT_EQ( controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); @@ -189,10 +243,13 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) TestableControllerInterfaceInitFailure controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); - ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), - controller_interface::return_type::ERROR); + controller_interface::ControllerInterfaceParams params; + params.controller_name = TEST_CONTROLLER_NAME; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::ERROR); ASSERT_EQ( controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 8efedd757b..6330532434 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -41,9 +41,13 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - ASSERT_EQ( - controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "controller_name"; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); @@ -70,9 +74,13 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters) controller_node_options.arguments( {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.", "-p", "parameter_list.parameter3:=3."}); - ASSERT_EQ( - controller.init("controller_name", "", 50.0, "", controller_node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "controller_name"; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller_node_options; + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); @@ -100,9 +108,13 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) std::cerr << params_file_path << std::endl; auto controller_node_options = controller.define_custom_node_options(); controller_node_options.arguments({"--ros-args", "--params-file", params_file_path}); - ASSERT_EQ( - controller.init("controller_name", "", 50.0, "", controller_node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "controller_name"; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller_node_options; + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); @@ -136,9 +148,13 @@ TEST( controller_node_options.arguments( {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785", "-p", "use_sim_time:=true"}); - ASSERT_EQ( - controller.init("controller_name", "", 50.0, "", controller_node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "controller_name"; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller_node_options; + ASSERT_EQ(controller.init(params), controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); @@ -165,9 +181,13 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - ASSERT_EQ( - controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), - controller_interface::return_type::ERROR); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "controller_name"; + params.robot_description = ""; + params.update_rate = 50; + params.node_namespace = ""; + params.node_options = controller.define_custom_node_options(); + ASSERT_EQ(controller.init(params), controller_interface::return_type::ERROR); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); diff --git a/controller_interface/test/test_force_torque_sensor.cpp b/controller_interface/test/test_force_torque_sensor.cpp index f1087a0758..8f8405054e 100644 --- a/controller_interface/test/test_force_torque_sensor.cpp +++ b/controller_interface/test/test_force_torque_sensor.cpp @@ -48,20 +48,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_y = std::make_shared( + sensor_name_, fts_interface_names_[1], &force_values_[1]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_x = std::make_shared( + sensor_name_, fts_interface_names_[3], &torque_values_[0]); + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; @@ -132,16 +132,16 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force.x and force.z - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque.y and torque.z - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; @@ -214,20 +214,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names) ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z"); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_y = std::make_shared( + sensor_name_, fts_interface_names_[1], &force_values_[1]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_x = std::make_shared( + sensor_name_, fts_interface_names_[3], &torque_values_[0]); + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_gps_sensor.cpp b/controller_interface/test/test_gps_sensor.cpp index 5358692e3b..e2cd748127 100644 --- a/controller_interface/test/test_gps_sensor.cpp +++ b/controller_interface/test/test_gps_sensor.cpp @@ -46,16 +46,21 @@ struct GPSSensorTest : public testing::Test gps_sensor_name}; std::vector full_interface_names; - hardware_interface::StateInterface gps_state{ - gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)}; - hardware_interface::StateInterface gps_service{ - gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)}; - hardware_interface::StateInterface latitude{ - gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)}; - hardware_interface::StateInterface longitude{ - gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)}; - hardware_interface::StateInterface altitude{ - gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)}; + hardware_interface::StateInterface::SharedPtr gps_state = + std::make_shared( + gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)); + hardware_interface::StateInterface::SharedPtr gps_service = + std::make_shared( + gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)); + hardware_interface::StateInterface::SharedPtr latitude = + std::make_shared( + gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)); + hardware_interface::StateInterface::SharedPtr longitude = + std::make_shared( + gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)); + hardware_interface::StateInterface::SharedPtr altitude = + std::make_shared( + gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)); std::vector state_interface; }; @@ -144,22 +149,30 @@ struct GPSSensorWithCovarianceTest : public testing::Test gps_sensor_name}; std::vector full_interface_names; - hardware_interface::StateInterface gps_state{ - gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)}; - hardware_interface::StateInterface gps_service{ - gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)}; - hardware_interface::StateInterface latitude{ - gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)}; - hardware_interface::StateInterface longitude{ - gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)}; - hardware_interface::StateInterface altitude{ - gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)}; - hardware_interface::StateInterface latitude_covariance{ - gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)}; - hardware_interface::StateInterface longitude_covariance{ - gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6)}; - hardware_interface::StateInterface altitude_covariance{ - gps_sensor_name, gps_interface_names.at(7), &gps_states.at(7)}; + hardware_interface::StateInterface::SharedPtr gps_state = + std::make_shared( + gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)); + hardware_interface::StateInterface::SharedPtr gps_service = + std::make_shared( + gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)); + hardware_interface::StateInterface::SharedPtr latitude = + std::make_shared( + gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)); + hardware_interface::StateInterface::SharedPtr longitude = + std::make_shared( + gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)); + hardware_interface::StateInterface::SharedPtr altitude = + std::make_shared( + gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)); + hardware_interface::StateInterface::SharedPtr latitude_covariance = + std::make_shared( + gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)); + hardware_interface::StateInterface::SharedPtr longitude_covariance = + std::make_shared( + gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6)); + hardware_interface::StateInterface::SharedPtr altitude_covariance = + std::make_shared( + gps_sensor_name, gps_interface_names.at(7), &gps_states.at(7)); std::vector state_interface; }; diff --git a/controller_interface/test/test_imu_sensor.cpp b/controller_interface/test/test_imu_sensor.cpp index f21a5a37ff..e6f8de44e5 100644 --- a/controller_interface/test/test_imu_sensor.cpp +++ b/controller_interface/test/test_imu_sensor.cpp @@ -47,30 +47,30 @@ TEST_F(IMUSensorTest, validate_all) std::vector interface_names = imu_sensor_->get_state_interface_names(); // assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, imu_interface_names_[0], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, imu_interface_names_[1], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, imu_interface_names_[2], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, imu_interface_names_[3], &orientation_values_[4]}; + auto orientation_x = std::make_shared( + sensor_name_, imu_interface_names_[0], &orientation_values_[0]); + auto orientation_y = std::make_shared( + sensor_name_, imu_interface_names_[1], &orientation_values_[1]); + auto orientation_z = std::make_shared( + sensor_name_, imu_interface_names_[2], &orientation_values_[2]); + auto orientation_w = std::make_shared( + sensor_name_, imu_interface_names_[3], &orientation_values_[4]); // assign values to angular velocity - hardware_interface::StateInterface angular_velocity_x{ - sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]}; - hardware_interface::StateInterface angular_velocity_y{ - sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]}; - hardware_interface::StateInterface angular_velocity_z{ - sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]}; + auto angular_velocity_x = std::make_shared( + sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]); + auto angular_velocity_y = std::make_shared( + sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]); + auto angular_velocity_z = std::make_shared( + sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]); // assign values to linear acceleration - hardware_interface::StateInterface linear_acceleration_x{ - sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]}; - hardware_interface::StateInterface linear_acceleration_y{ - sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]}; - hardware_interface::StateInterface linear_acceleration_z{ - sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]}; + auto linear_acceleration_x = std::make_shared( + sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]); + auto linear_acceleration_y = std::make_shared( + sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]); + auto linear_acceleration_z = std::make_shared( + sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_led_rgb_device.cpp b/controller_interface/test/test_led_rgb_device.cpp index 9f39726fa4..4be4242b1e 100644 --- a/controller_interface/test/test_led_rgb_device.cpp +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -47,9 +47,12 @@ TEST_F(LedDeviceTest, validate_all) std::vector interface_names = led_device_->get_command_interface_names(); // Assign values to position - hardware_interface::CommandInterface led_r{device_name_, interface_names_[0], &led_values_[0]}; - hardware_interface::CommandInterface led_g{device_name_, interface_names_[1], &led_values_[1]}; - hardware_interface::CommandInterface led_b{device_name_, interface_names_[2], &led_values_[2]}; + auto led_r = std::make_shared( + device_name_, interface_names_[0], &led_values_[0]); + auto led_g = std::make_shared( + device_name_, interface_names_[1], &led_values_[1]); + auto led_b = std::make_shared( + device_name_, interface_names_[2], &led_values_[2]); // Create command interface vector in jumbled order std::vector temp_command_interfaces; diff --git a/controller_interface/test/test_magnetic_field_sensor.cpp b/controller_interface/test/test_magnetic_field_sensor.cpp new file mode 100644 index 0000000000..629dbb2dd8 --- /dev/null +++ b/controller_interface/test/test_magnetic_field_sensor.cpp @@ -0,0 +1,82 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_magnetic_field_sensor.hpp" + +#include +#include +#include +#include "sensor_msgs/msg/magnetic_field.hpp" + +void MagneticFieldSensorTest::TearDown() { magnetic_field_sensor_.reset(nullptr); } + +TEST_F(MagneticFieldSensorTest, validate_all) +{ + // Create the magnetic field sensor + magnetic_field_sensor_ = std::make_unique(sensor_name_); + + // validate the component name + ASSERT_EQ(magnetic_field_sensor_->name_, sensor_name_); + + // Validate the space reserved for interface_names_ and state_interfaces_ + // Note : Using capacity() for state_interfaces_ as no such interfaces are defined yet + ASSERT_EQ(magnetic_field_sensor_->interface_names_.size(), size_); + ASSERT_EQ(magnetic_field_sensor_->state_interfaces_.capacity(), size_); + + // Validate the default interface_names_ + ASSERT_TRUE( + std::equal( + magnetic_field_sensor_->interface_names_.begin(), + magnetic_field_sensor_->interface_names_.end(), full_interface_names_.begin(), + full_interface_names_.end())); + + // Get the interface names + std::vector interface_names = magnetic_field_sensor_->get_state_interface_names(); + + // Assign values + auto magnetic_field_x = std::make_shared( + sensor_name_, magnetic_field_interface_names_[0], &magnetic_field_values_[0]); + auto magnetic_field_y = std::make_shared( + sensor_name_, magnetic_field_interface_names_[1], &magnetic_field_values_[1]); + auto magnetic_field_z = std::make_shared( + sensor_name_, magnetic_field_interface_names_[2], &magnetic_field_values_[2]); + + // Create local state interface vector + std::vector temp_state_interfaces; + temp_state_interfaces.reserve(10); + + // Insert the interfaces in jumbled sequence + temp_state_interfaces.emplace_back(magnetic_field_y); + temp_state_interfaces.emplace_back(magnetic_field_z); + temp_state_interfaces.emplace_back(magnetic_field_x); + + // Now call the function to make them in order like interface_names + magnetic_field_sensor_->assign_loaned_state_interfaces(temp_state_interfaces); + + // Validate the count of state_interfaces_ + ASSERT_EQ(magnetic_field_sensor_->state_interfaces_.size(), size_); + + // Validate get_values_as_message() + sensor_msgs::msg::MagneticField temp_message; + ASSERT_TRUE(magnetic_field_sensor_->get_values_as_message(temp_message)); + ASSERT_EQ(temp_message.magnetic_field.x, magnetic_field_values_[0]); + ASSERT_EQ(temp_message.magnetic_field.y, magnetic_field_values_[1]); + ASSERT_EQ(temp_message.magnetic_field.z, magnetic_field_values_[2]); + + // release the state_interfaces_ + magnetic_field_sensor_->release_interfaces(); + + // validate the count of state_interfaces_ + ASSERT_EQ(magnetic_field_sensor_->state_interfaces_.size(), 0u); +} diff --git a/controller_interface/test/test_magnetic_field_sensor.hpp b/controller_interface/test/test_magnetic_field_sensor.hpp new file mode 100644 index 0000000000..84e31cf9f4 --- /dev/null +++ b/controller_interface/test/test_magnetic_field_sensor.hpp @@ -0,0 +1,63 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_MAGNETIC_FIELD_SENSOR_HPP_ +#define TEST_MAGNETIC_FIELD_SENSOR_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" +#include "semantic_components/magnetic_field_sensor.hpp" + +// implementing and friending so we can access member variables +class TestableMagneticFieldSensor : public semantic_components::MagneticFieldSensor +{ + FRIEND_TEST(MagneticFieldSensorTest, validate_all); + +public: + // Use generation of interface names + explicit TestableMagneticFieldSensor(const std::string & name) : MagneticFieldSensor(name) {} + + virtual ~TestableMagneticFieldSensor() = default; +}; + +class MagneticFieldSensorTest : public ::testing::Test +{ +public: + void SetUp() + { + full_interface_names_.reserve(size_); + for (auto index = 0u; index < size_; ++index) + { + full_interface_names_.emplace_back( + sensor_name_ + "/" + magnetic_field_interface_names_[index]); + } + } + + void TearDown(); + +protected: + const size_t size_ = 3; + const std::string sensor_name_ = "test_magnetometer"; + std::array magnetic_field_values_ = {{4.4, 5.5, 6.6}}; + std::unique_ptr magnetic_field_sensor_; + + std::vector full_interface_names_; + const std::vector magnetic_field_interface_names_ = { + "magnetic_field.x", "magnetic_field.y", "magnetic_field.z"}; +}; + +#endif // TEST_MAGNETIC_FIELD_SENSOR_HPP_ diff --git a/controller_interface/test/test_pose_sensor.cpp b/controller_interface/test/test_pose_sensor.cpp index 6a06f2f224..6d9cf2dce9 100644 --- a/controller_interface/test/test_pose_sensor.cpp +++ b/controller_interface/test/test_pose_sensor.cpp @@ -45,22 +45,22 @@ TEST_F(PoseSensorTest, validate_all) std::vector interface_names = pose_sensor_->get_state_interface_names(); // Assign values to position - hardware_interface::StateInterface position_x{ - sensor_name_, interface_names_[0], &position_values_[0]}; - hardware_interface::StateInterface position_y{ - sensor_name_, interface_names_[1], &position_values_[1]}; - hardware_interface::StateInterface position_z{ - sensor_name_, interface_names_[2], &position_values_[2]}; + auto position_x = std::make_shared( + sensor_name_, interface_names_[0], &position_values_[0]); + auto position_y = std::make_shared( + sensor_name_, interface_names_[1], &position_values_[1]); + auto position_z = std::make_shared( + sensor_name_, interface_names_[2], &position_values_[2]); // Assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, interface_names_[3], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, interface_names_[4], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, interface_names_[5], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, interface_names_[6], &orientation_values_[3]}; + auto orientation_x = std::make_shared( + sensor_name_, interface_names_[3], &orientation_values_[0]); + auto orientation_y = std::make_shared( + sensor_name_, interface_names_[4], &orientation_values_[1]); + auto orientation_z = std::make_shared( + sensor_name_, interface_names_[5], &orientation_values_[2]); + auto orientation_w = std::make_shared( + sensor_name_, interface_names_[6], &orientation_values_[3]); // Create state interface vector in jumbled order std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_semantic_component_command_interface.cpp b/controller_interface/test/test_semantic_component_command_interface.cpp index 07d4d54f5e..1251e5f4d5 100644 --- a/controller_interface/test/test_semantic_component_command_interface.cpp +++ b/controller_interface/test/test_semantic_component_command_interface.cpp @@ -38,9 +38,12 @@ TEST_F(SemanticCommandInterfaceTest, validate_command_interfaces) std::vector interface_values = { std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; - hardware_interface::CommandInterface cmd_interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::CommandInterface cmd_interface_2{component_name_, "2", &interface_values[1]}; - hardware_interface::CommandInterface cmd_interface_3{component_name_, "3", &interface_values[2]}; + auto cmd_interface_1 = std::make_shared( + component_name_, "1", &interface_values[0]); + auto cmd_interface_2 = std::make_shared( + component_name_, "2", &interface_values[1]); + auto cmd_interface_3 = std::make_shared( + component_name_, "3", &interface_values[2]); // create local command interface vector std::vector temp_command_interfaces; diff --git a/controller_interface/test/test_semantic_component_interface.cpp b/controller_interface/test/test_semantic_component_interface.cpp index 8a7817c2a3..1a0c0aa2b1 100644 --- a/controller_interface/test/test_semantic_component_interface.cpp +++ b/controller_interface/test/test_semantic_component_interface.cpp @@ -95,9 +95,12 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) std::vector interface_values = {1.1, 3.3, 5.5}; // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5 - hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]}; - hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]}; + auto interface_1 = std::make_shared( + component_name_, "1", &interface_values[0]); + auto interface_3 = std::make_shared( + component_name_, "3", &interface_values[1]); + auto interface_5 = std::make_shared( + component_name_, "5", &interface_values[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 812b40fc02..ad6ab672e1 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,95 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ +* Fail early for the resource conflicts between the activating controllers (`#2760 `_) +* Contributors: Sai Kishor Kothakota + +6.0.0 (2025-10-27) +------------------ +* Switch the default strictness to STRICT (`#2742 `_) +* Deactivate the whole controller chain if one of the update results in ERROR (`#2681 `_) +* Deactivate the controller chain upon failed group activation (`#2669 `_) +* Add test_test_utils to CMakeLists (`#2729 `_) +* Fix concurrent spinning of the test_node (`#2721 `_) +* [Spawner] Fix failing makedirs on multiple spawners at startup (`#2698 `_) +* [Spawner] Create the FileLock in the ROS_HOME location (`#2677 `_) +* Fix the same hardware component node naming issue with multiple controller managers setup (`#2657 `_) +* Move clock availability check to controller manager thread (`#2654 `_) +* increase tolerance of the controller manager tests (`#2629 `_) +* [Controllers] Set async thread properties via parameters (`#2613 `_) +* Contributors: Christoph Fröhlich, Erik Holum, Sai Kishor Kothakota + +5.7.0 (2025-10-03) +------------------ +* [CM] Ability to switch controllers in non-realtime loop (`#2452 `_) +* Added parameters to handle the overruns behaviour and prints (`#2546 `_) +* [Controllers] Receive a valid period on the first update cycle (`#2572 `_) +* Add `ControllerInterfaceParams` to initialize the Controllers (`#2390 `_) +* Add parameter to allow controllers with inactive hardware components (`#2501 `_) +* Fix exclusive hardware control mode switching on controller failed activation (`#1522 `_) +* Fix the skipped cycles of controllers running at different rate (`#2557 `_) +* Fix shadowed variables, redefinition and old-style casts (`#2569 `_) +* [Spawner] Release the lock while waiting for the interrupt (`#2559 `_) +* Fix the reloading controller with failed activation (`#2544 `_) +* Don't print the overrun warnings for the simulations (`#2539 `_) +* Readd the ResourceManagerParams initialization to ControllerManager (`#2522 `_) +* Publish controller manager statistics to better introspect the timings (`#2449 `_) +* Fix the CPU affinity of the ros2_control_node (`#2509 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ +* Fix the prepare_command_mode_switch behaviour when HW is INACTIVE (`#2347 `_) +* Contributors: Sai Kishor Kothakota + +5.4.0 (2025-07-21) +------------------ +* Increase controller period tolerance further (`#2405 `_) +* Fix controller activation crash on macOS (Fixes `#604 `_) (`#2391 `_) +* Increase controller period tolerance in tests (`#2388 `_) +* [spawner] Fix Lock timeout error crashes (`#2386 `_) +* [Spawner] Fix the scope issue of the logger (`#2382 `_) +* Increase tolerance to improve the success rate of the tests (`#2373 `_) +* [Spawner] Change strategy for `--unload-on-kill` option (`#2372 `_) +* Contributors: Dhruv Patel, Jasper van Brakel, Sai Kishor Kothakota + +5.3.0 (2025-07-02) +------------------ +* Cleanup old internal API (`#2346 `_) +* added params approach to allow propagation in gz_ros2_control (`#2340 `_) +* Deactivate controllers with command interfaces to hardware on DEACTIVATE (`#2334 `_) +* Shift to Struct based Method and Constructors, with Executor passed from CM to `on_init()` (`#2323 `_) +* Contributors: Marq Rasmussen, Sai Kishor Kothakota, Soham Patil + +5.2.0 (2025-06-07) +------------------ +* Move `enforce_command_limits` parameter to GPL parameters (`#2305 `_) +* Cleanup test name (`#2295 `_) +* check_controllers_running: Make timeout a parameter (`#2278 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +5.1.0 (2025-05-24) +------------------ +* Use target_link_libraries instead of ament_target_dependencies (`#2266 `_) +* Cleanup deprecations in `ros_control` (`#2258 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* [CM] Add option to avoid shutting down on hardware initial state failure (`#2230 `_) +* Statically allocate string concatenations using FMT formatting (`#2205 `_) +* Suppress the deprecation warnings of the hardware_interface API (`#2223 `_) +* Use `warning` attribute of RcutilsLogger (`#2244 `_) +* [CM] Set default strictness of switch_controllers using parameters (`#2168 `_) +* Add `data_type` field to the HardwareInterfaces message (`#2204 `_) +* Add new strictness modes to SwitchController service (`#2224 `_) +* Contributors: Marq Rasmussen, Sai Kishor Kothakota, mini-1235 + 4.29.0 (2025-05-04) ------------------- * Use the new reusable helpers methods in hardware diagnostics (`#2216 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0a2b103fbc..739fdc020d 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS std_msgs libstatistics_collector generate_parameter_library + fmt ) find_package(ament_cmake REQUIRED) @@ -40,9 +41,18 @@ 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}) + controller_manager_parameters + controller_interface::controller_interface + diagnostic_updater::diagnostic_updater + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + libstatistics_collector::libstatistics_collector + fmt::fmt + ${std_msgs_TARGETS} + ${controller_manager_msgs_TARGETS}) add_executable(ros2_control_node src/ros2_control_node.cpp) target_link_libraries(ros2_control_node PRIVATE @@ -53,11 +63,13 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) find_package(example_interfaces REQUIRED) + find_package(ament_cmake_gtest 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_link_libraries(test_controller PUBLIC + controller_manager + ${example_interfaces_TARGETS}) pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) install( TARGETS test_controller @@ -78,8 +90,9 @@ if(BUILD_TESTING) add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp ) - ament_target_dependencies(test_chainable_controller PUBLIC realtime_tools) - target_link_libraries(test_chainable_controller PUBLIC controller_manager) + target_link_libraries(test_chainable_controller PUBLIC + controller_manager + realtime_tools::realtime_tools) pluginlib_export_plugin_description_file( controller_interface test/test_chainable_controller/test_chainable_controller.xml) install( @@ -146,11 +159,9 @@ if(BUILD_TESTING) test_controller test_chainable_controller ros2_control_test_assets::ros2_control_test_assets + ${controller_manager_msgs_TARGETS} ) set_tests_properties(test_controller_manager_srvs PROPERTIES TIMEOUT 120) - ament_target_dependencies(test_controller_manager_srvs - controller_manager_msgs - ) ament_add_gmock(test_controller_manager_urdf_passing test/test_controller_manager_urdf_passing.cpp ) @@ -158,9 +169,7 @@ if(BUILD_TESTING) controller_manager test_controller ros2_control_test_assets::ros2_control_test_assets - ) - ament_target_dependencies(test_controller_manager_urdf_passing - controller_manager_msgs + ${controller_manager_msgs_TARGETS} ) add_library(test_controller_with_interfaces SHARED @@ -176,6 +185,20 @@ if(BUILD_TESTING) DESTINATION lib ) + add_library(test_controller_failed_activate SHARED + test/test_controller_failed_activate/test_controller_failed_activate.cpp + ) + target_link_libraries(test_controller_failed_activate PUBLIC + controller_manager + ) + target_compile_definitions(test_controller_failed_activate PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller_failed_activate/test_controller_failed_activate.xml) + install( + TARGETS test_controller_failed_activate + DESTINATION lib + ) + ament_add_gmock(test_release_interfaces test/test_release_interfaces.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ @@ -184,6 +207,7 @@ if(BUILD_TESTING) controller_manager test_controller test_controller_with_interfaces + test_controller_failed_activate ros2_control_test_assets::ros2_control_test_assets ) @@ -217,15 +241,23 @@ if(BUILD_TESTING) controller_manager test_controller ros2_control_test_assets::ros2_control_test_assets + ${controller_manager_msgs_TARGETS} ) - ament_target_dependencies(test_hardware_management_srvs - controller_manager_msgs + + ament_add_gtest(test_controller_manager_with_resource_manager + test/test_controller_manager_with_resource_manager.cpp + ) + target_link_libraries(test_controller_manager_with_resource_manager + controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) find_package(ament_cmake_pytest REQUIRED) install(FILES test/test_ros2_control_node.yaml DESTINATION test) ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) + ament_add_pytest_test(test_test_utils test/test_test_utils.py) endif() install( diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 8fa7452ad3..dc2647993f 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -133,7 +133,7 @@ def service_caller( f"Could not contact service {fully_qualified_service_name}" ) elif not cli.wait_for_service(10.0): - node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}") + node.get_logger().warning(f"Could not contact service {fully_qualified_service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = None @@ -275,7 +275,7 @@ def switch_controllers( controller_manager_name, deactivate_controllers, activate_controllers, - strict, + strictness, activate_asap, timeout, call_timeout=10.0, @@ -283,10 +283,7 @@ def switch_controllers( request = SwitchController.Request() request.activate_controllers = activate_controllers request.deactivate_controllers = deactivate_controllers - if strict: - request.strictness = SwitchController.Request.STRICT - else: - request.strictness = SwitchController.Request.BEST_EFFORT + request.strictness = strictness request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() return service_caller( diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 4f7afe714c..abba851432 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -70,11 +70,11 @@ def handle_set_component_state_service_call( f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}" ) elif response.ok and not response.state == target_state: - node.get_logger().warn( + node.get_logger().warning( f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}" ) else: - node.get_logger().warn( + node.get_logger().warning( f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}" ) @@ -156,7 +156,7 @@ def main(args=None): if not is_hardware_component_loaded( node, controller_manager_name, hardware_component, controller_manager_timeout ): - node.get_logger().warn( + node.get_logger().warning( f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}" ) elif activate: diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 2502f0f534..38628d0af8 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -30,6 +30,7 @@ set_controller_parameters_from_param_files, bcolors, ) +from controller_manager_msgs.srv import SwitchController from controller_manager.controller_manager_services import ServiceNotFoundError from filelock import Timeout, FileLock @@ -92,13 +93,6 @@ def main(args=None): action="append", required=False, ) - parser.add_argument( - "-n", - "--namespace", - help="DEPRECATED Namespace for the controller_manager and the controller(s)", - default=None, - required=False, - ) parser.add_argument( "--load-only", help="Only load the controller and leave unconfigured.", @@ -147,6 +141,13 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--switch-asap", + help="Option to switch the controllers in the realtime loop at the earliest possible time or in the non-realtime loop.", + required=False, + default=False, + action=argparse.BooleanOptionalAction, + ) parser.add_argument( "--controller-ros-args", help="The --ros-args to be passed to the controller node, e.g., for remapping topics. " @@ -164,57 +165,56 @@ def main(args=None): controller_manager_timeout = args.controller_manager_timeout service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout + strictness = SwitchController.Request.STRICT + unload_controllers_upon_exit = False + switch_asap = args.switch_asap + node = None 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) + logger = rclpy.logging.get_logger("ros2_control_controller_spawner_" + controller_names[0]) try: spawner_node_name = "spawner_" + controller_names[0] - lock = FileLock("/tmp/ros2-control-controller-spawner.lock") + # Get the environment variable $ROS_HOME or default to ~/.ros + ros_home = os.getenv("ROS_HOME", os.path.join(os.path.expanduser("~"), ".ros")) + ros_control_lock_dir = os.path.join(ros_home, "locks") + if not os.path.exists(ros_control_lock_dir): + try: + os.makedirs(ros_control_lock_dir) + except FileExistsError: + pass + lock = FileLock(f"{ros_control_lock_dir}/ros2-control-controller-spawner.lock") max_retries = 5 retry_delay = 3 # seconds for attempt in range(max_retries): - tmp_logger = rclpy.logging.get_logger(spawner_node_name) try: - tmp_logger.debug( + logger.debug( bcolors.OKGREEN + "Waiting for the spawner lock to be acquired!" + bcolors.ENDC ) # timeout after 20 seconds and try again lock.acquire(timeout=20) - tmp_logger.debug(bcolors.OKGREEN + "Spawner lock acquired!" + bcolors.ENDC) + logger.debug(bcolors.OKGREEN + "Spawner lock acquired!" + bcolors.ENDC) break except Timeout: - tmp_logger.warn( + logger.warning( bcolors.WARNING + f"Attempt {attempt+1} failed. Retrying in {retry_delay} seconds..." + bcolors.ENDC ) time.sleep(retry_delay) else: - tmp_logger.error( - bcolors.ERROR + "Failed to acquire lock after multiple attempts." + bcolors.ENDC + logger.error( + bcolors.FAIL + "Failed to acquire lock after multiple attempts." + bcolors.ENDC ) return 1 node = Node(spawner_node_name) + logger = node.get_logger() - if node.get_namespace() != "/" and args.namespace: - raise RuntimeError( - f"Setting namespace through both '--namespace {args.namespace}' arg and the ROS 2 standard way " - f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!" - ) - - if args.namespace: - warnings.filterwarnings("always") - warnings.warn( - "The '--namespace' argument is deprecated and will be removed in future releases." - " Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=", - DeprecationWarning, - ) - - spawner_namespace = args.namespace if args.namespace else node.get_namespace() + spawner_namespace = node.get_namespace() if not spawner_namespace.startswith("/"): spawner_namespace = f"/{spawner_namespace}" @@ -234,7 +234,7 @@ def main(args=None): controller_manager_timeout, service_call_timeout, ): - node.get_logger().warn( + logger.warning( bcolors.WARNING + "Controller already loaded, skipping load_controller" + bcolors.ENDC @@ -267,7 +267,7 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - node.get_logger().fatal( + logger.fatal( bcolors.FAIL + "Failed loading controller " + bcolors.BOLD @@ -275,7 +275,7 @@ def main(args=None): + bcolors.ENDC ) return 1 - node.get_logger().info( + logger.info( bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC ) @@ -288,9 +288,7 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - node.get_logger().error( - bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC - ) + logger.error(bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC) return 1 if not args.inactive and not args.activate_as_group: @@ -299,18 +297,18 @@ def main(args=None): controller_manager_name, [], [controller_name], - True, - True, + strictness, + switch_asap, switch_timeout, service_call_timeout, ) if not ret.ok: - node.get_logger().error( + logger.error( f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}" ) return 1 - node.get_logger().info( + logger.info( bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD @@ -324,54 +322,52 @@ def main(args=None): controller_manager_name, [], controller_names, - True, - True, + strictness, + switch_asap, switch_timeout, service_call_timeout, ) if not ret.ok: - node.get_logger().error( + logger.error( f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}" ) return 1 - node.get_logger().info( + logger.info( bcolors.OKGREEN + f"Configured and activated all the parsed controllers list : {controller_names}!" + bcolors.ENDC ) - - if not args.unload_on_kill: + unload_controllers_upon_exit = args.unload_on_kill + if not unload_controllers_upon_exit: return 0 - try: - node.get_logger().info("Waiting until interrupt to unload controllers") - while True: - time.sleep(1) - except KeyboardInterrupt: - node.get_logger().info("KeyboardInterrupt successfully captured!") + # The lock has to be released to not block other spawner instances while waiting for the interrupt + lock.release() + logger.info("Waiting until interrupt to unload controllers") + while True: + time.sleep(1) + except KeyboardInterrupt: + if unload_controllers_upon_exit: + logger.info("KeyboardInterrupt successfully captured!") if not args.inactive: - node.get_logger().info("Deactivating and unloading controllers...") + logger.info("Deactivating and unloading controllers...") # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode ret = switch_controllers( node, controller_manager_name, controller_names, [], - True, - True, + strictness, + switch_asap, switch_timeout, service_call_timeout, ) if not ret.ok: - node.get_logger().error( - bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC - ) + logger.error(bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC) return 1 - node.get_logger().info( - f"Successfully deactivated controllers : {controller_names}" - ) + logger.info(f"Successfully deactivated controllers : {controller_names}") unload_status = True for controller_name in controller_names: @@ -384,26 +380,27 @@ def main(args=None): ) if not ret.ok: unload_status = False - node.get_logger().error( + logger.error( bcolors.FAIL + f"Failed to unload controller : {controller_name}" + bcolors.ENDC ) if unload_status: - node.get_logger().info(f"Successfully unloaded controllers : {controller_names}") + logger.info(f"Successfully unloaded controllers : {controller_names}") else: return 1 - return 0 - except KeyboardInterrupt: - node.get_logger().info("KeyboardInterrupt received! Exiting....") - pass + else: + logger.info("KeyboardInterrupt received! Exiting....") + pass except ServiceNotFoundError as err: - node.get_logger().fatal(str(err)) + logger.fatal(str(err)) return 1 finally: - node.destroy_node() - lock.release() + if node: + node.destroy_node() + if lock.is_locked: + lock.release() rclpy.shutdown() diff --git a/controller_manager/controller_manager/test_utils.py b/controller_manager/controller_manager/test_utils.py index 8fb4ef8d97..c5b90b4ee4 100644 --- a/controller_manager/controller_manager/test_utils.py +++ b/controller_manager/controller_manager/test_utils.py @@ -36,6 +36,17 @@ def check_node_running(node, node_name, timeout=5.0): + """ + Checks if a node with the specified name is running within a given timeout. + + Args: + node (Node): The ROS 2 node instance to use for discovering nodes. + node_name (str): The name of the node to check for. + timeout (float, optional): The maximum time to wait for the node to appear, in seconds. Defaults to 5.0. + + Raises: + AssertionError: If the node with the specified name is not found within the timeout period. + """ start = time.time() found = False @@ -45,15 +56,16 @@ def check_node_running(node, node_name, timeout=5.0): assert found, f"{node_name} not found!" -def check_controllers_running(node, cnames, namespace="", state="active"): +def check_controllers_running(node, cnames, namespace="", state="active", timeout=10.0): """ Check if the specified controllers are running on the given node. Args: - node (Node): The ROS2 node instance to check for controllers. + node (Node): The ROS 2 node instance to use for discovering controllers. cnames (list of str): List of controller names to check. namespace (str, optional): The namespace in which to look for controllers. Defaults to "". state (str, optional): The desired state of the controllers. Defaults to "active". + timeout (float, optional): The maximum time to wait for the node to appear, in seconds. Defaults to 10.0. Raises: AssertionError: If any of the specified controllers are not found or not in the desired state within the timeout period. @@ -72,7 +84,7 @@ def check_controllers_running(node, cnames, namespace="", state="active"): else: namespace_api = "/" - while time.time() - start < 10.0 and not all(found.values()): + while time.time() - start < timeout and not all(found.values()): node_names_namespaces = node.get_node_names_and_namespaces() for cname in cnames: if any(name == cname and ns == namespace_api for name, ns in node_names_namespaces): @@ -92,7 +104,7 @@ def check_controllers_running(node, cnames, namespace="", state="active"): cm = namespace + "controller_manager" else: cm = namespace + "/controller_manager" - while time.time() - start < 10.0 and not all(found.values()): + while time.time() - start < timeout and not all(found.values()): controllers = list_controllers(node, cm, 5.0).controller assert controllers, "No controllers found!" for c in controllers: diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 9e380f5086..f715936138 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -19,6 +19,7 @@ import warnings from controller_manager import switch_controllers, unload_controller +from controller_manager_msgs.srv import SwitchController from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy @@ -51,6 +52,7 @@ def main(args=None): controller_names = args.controller_names controller_manager_name = args.controller_manager switch_timeout = args.switch_timeout + strictness = SwitchController.Request.STRICT node = Node("unspawner_" + controller_names[0]) try: @@ -60,7 +62,7 @@ def main(args=None): controller_manager_name, controller_names, [], - True, + strictness, True, switch_timeout, ) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 9e720fc681..b5566b72a7 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -79,11 +79,6 @@ robot_description [std_msgs::msg::String] Parameters ----------- -enforce_command_limits (optional; bool; default: ``false`` for Jazzy and earlier distro and ``true`` for Rolling and newer distros) - Enforces the joint limits to the joint command interfaces. - If the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits in the URDF. - If the command is within the limits, the command is passed through without any changes. - .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. @@ -168,9 +163,9 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h - usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--controller-ros-args CONTROLLER_ROS_ARGS] - controller_names [controller_names ...] + usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] + [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group] [--switch-asap | --no-switch-asap] [--controller-ros-args CONTROLLER_ROS_ARGS] + controller_names [controller_names ...] positional arguments: controller_names List of controllers @@ -181,21 +176,20 @@ There are two scripts to interact with controller manager from launch files: 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. 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 service to be available + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful when switching cannot be performed immediately, e.g., paused simulations at startup --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 + --switch-asap, --no-switch-asap + Option to switch the controllers in the realtime loop at the earliest possible time or in the non-realtime loop. --controller-ros-args CONTROLLER_ROS_ARGS - The --ros-args to be passed to the controller node for remapping topics etc + The --ros-args to be passed to the controller node, e.g., for remapping topics. Pass multiple times for every argument. 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: @@ -391,6 +385,12 @@ thread_priority (optional; int; default: 50) use_sim_time (optional; bool; default: false) Enables the use of simulation time in the ``controller_manager`` node. +overruns.manage (optional; bool; default: true) + Enables or disables the handling of overruns in the real-time loop of the ``controller_manager`` node. + If set to true, the controller manager will detect overruns caused by system time changes or longer execution times of the controllers and hardware components. + If an overrun is detected, the controller manager will print a warning message to the console. + When used with ``use_sim_time`` set to true, this parameter is ignored and the overrun handling is disabled. + Concepts ----------- @@ -409,7 +409,14 @@ Hardware and Controller Errors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces. -Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available. +Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller (or) the entire controller chain it is part of, then the controller manager will try to start any available fallback controllers. + +Factors that affect Determinism +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +When run under the conditions determined in the above section, the determinism is assured up to the limitations of the hardware and the real-time kernel. However, there are some situations that can affect determinism: + +* When a controller fails to activate in the realtime loop, the controller_manager will call the methods ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the started interfaces. These calls can cause jitter in the main control loop. +* If a controller does not complete a successful update cycle in the realtime loop (for example, returns ``return_type::ERROR``), the controller manager will deactivate that controller (or) the entire controller chain it is part of. It will then invoke ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the interfaces used by the affected controller(s). These actions can introduce jitter into the main control loop. Support for Asynchronous Updates ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -431,6 +438,21 @@ The async update support is transparent to each controller implementation. A con update_rate: 20 # Hz ... +You can set more parameters of the ``AsyncFunctionHandler``, which handles the thread of the controller, using the ``async_parameters`` namespace. For example: + +.. code-block:: yaml + + example_async_controller: + ros__parameters: + type: example_controller/ExampleAsyncController + update_rate: 20 # Hz + is_async: true + async_parameters: + cpu_affinity: [2, 4] + thread_priority: 50 + wait_until_initial_trigger: false + ... + will result in the controller being loaded and configured to run at 20Hz, while the controller manager runs at 100Hz. The description of the parameters can be found in the `Common Controller Parameters `_ section of the ros2_controllers documentation. Scheduling Behavior diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 671b6f19a2..259bee233b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -40,6 +40,7 @@ #include "controller_manager_msgs/srv/unload_controller.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" +#include "hardware_interface/helpers.hpp" #include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" @@ -51,7 +52,7 @@ namespace controller_manager { class ParamListener; -class Params; +struct Params; using ControllersListIterator = std::vector::const_iterator; rclcpp::NodeOptions get_cm_node_options(); @@ -241,6 +242,17 @@ class ControllerManager : public rclcpp::Node */ rclcpp::Clock::SharedPtr get_trigger_clock() const; + /** + * @brief Return the robot description timer. + * + * It can be used to determine whether the Controller Manager + * is currently waiting for a robot description to be published. + * + * @return rclcpp::TimerBase::SharedPtr if the timer exists, nullptr otherwise + */ + rclcpp::TimerBase::SharedPtr get_robot_description_timer() const { + return robot_description_notification_timer_; + } protected: void init_services(); @@ -259,7 +271,7 @@ class ControllerManager : public rclcpp::Node */ void deactivate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_deactivate); + const std::vector & controllers_to_deactivate); /** * Switch chained mode for all the controllers with respect to the following cases: @@ -279,25 +291,11 @@ class ControllerManager : public rclcpp::Node * * \param[in] rt_controller_list controllers in the real-time list. * \param[in] controllers_to_activate names of the controller that have to be activated. + * \param[in] strictness level of strictness for activation. */ void activate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_activate); - - /// Activate chosen controllers from real-time controller list. - /** - * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. - * The controller list will be iterated as many times as there are controller names. - * - * *NOTE*: There is currently not difference to `activate_controllers` method. - * Check https://github.com/ros-controls/ros2_control/issues/263 for more information. - * - * \param[in] rt_controller_list controllers in the real-time list. - * \param[in] controllers_to_activate names of the controller that have to be activated. - */ - void activate_controllers_asap( - const std::vector & rt_controller_list, - const std::vector controllers_to_activate); + const std::vector & controllers_to_activate, int strictness); void list_controllers_srv_cb( const std::shared_ptr request, @@ -471,12 +469,13 @@ class ControllerManager : public rclcpp::Node * * \param[in] controllers list with controllers. * \param[in] activation_list list with controllers to activate. + * \param[in] deactivation_list list with controllers to deactivate. * \param[out] message describing the result of the check. * \return return_type::OK if all interfaces are available, otherwise return_type::ERROR. */ controller_interface::return_type check_for_interfaces_availability_to_activate( const std::vector & controllers, const std::vector activation_list, - std::string & message); + const std::vector deactivation_list, std::string & message); /** * @brief Inserts a controller into an ordered list based on dependencies to compute the @@ -501,6 +500,14 @@ class ControllerManager : public rclcpp::Node const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller); + /** + * @brief Build the controller chain topology information based on the provided controllers. + * This method constructs a directed graph representing the dependencies between controllers. + * It analyzes the relationships between controllers, such as which controllers depend on others, + * and builds a directed graph to represent these dependencies. + */ + void build_controllers_topology_info(const std::vector & controllers); + /** * @brief Method to publish the state of the controller manager. * The state includes the list of controllers and the list of hardware interfaces along with @@ -630,6 +637,7 @@ class ControllerManager : public rclcpp::Node std::function on_switch_callback_ = nullptr; }; + bool use_sim_time_; rclcpp::Clock::SharedPtr trigger_clock_ = nullptr; std::unique_ptr preshutdown_cb_handle_{nullptr}; RTControllerListWrapper rt_controllers_wrapper_; @@ -661,11 +669,6 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr set_hardware_component_state_service_; - std::vector activate_request_, deactivate_request_; - std::vector to_chained_mode_request_, from_chained_mode_request_; - std::vector activate_command_interface_request_, - deactivate_command_interface_request_; - std::map> controller_chained_reference_interfaces_cache_; std::map> controller_chained_state_interfaces_cache_; @@ -673,7 +676,21 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; - bool enforce_command_limits_; + + struct ControllerManagerExecutionTime + { + double read_time = 0.0; + double update_time = 0.0; + double write_time = 0.0; + double switch_time = 0.0; + double total_time = 0.0; + double switch_chained_mode_time = 0.0; + double switch_perform_mode_time = 0.0; + double deactivation_time = 0.0; + double activation_time = 0.0; + }; + + ControllerManagerExecutionTime execution_time_; controller_manager::MovingAverageStatistics periodicity_stats_; @@ -687,17 +704,34 @@ class ControllerManager : public rclcpp::Node activate_asap = false; } - bool do_switch; + std::atomic_bool do_switch; bool started; // Switch options int strictness; - bool activate_asap; + std::atomic_bool activate_asap; std::chrono::nanoseconds timeout; // conditional variable and mutex to wait for the switch to complete std::condition_variable cv; std::mutex mutex; + + bool skip_cycle(const controller_manager::ControllerSpec & spec) const + { + const std::string controller_name = spec.info.name; + return ros2_control::has_item(activate_request, controller_name) || + ros2_control::has_item(deactivate_request, controller_name) || + ros2_control::has_item(to_chained_mode_request, controller_name) || + ros2_control::has_item(from_chained_mode_request, controller_name); + } + + // The controllers list to activate and deactivate + std::vector activate_request; + std::vector deactivate_request; + std::vector to_chained_mode_request; + std::vector from_chained_mode_request; + std::vector activate_command_interface_request; + std::vector deactivate_command_interface_request; }; SwitchParams switch_params_; diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9cc508772d..cc3db440ee 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -24,13 +24,12 @@ #include #include "controller_interface/controller_interface_base.hpp" #include "hardware_interface/controller_info.hpp" -#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" +#include "hardware_interface/types/statistics_types.hpp" namespace controller_manager { -using MovingAverageStatistics = - libstatistics_collector::moving_average_statistics::MovingAverageStatistics; +using MovingAverageStatistics = ros2_control::MovingAverageStatistics; /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -49,6 +48,7 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; + std::vector controllers_chain_group = {}; std::shared_ptr execution_time_statistics; std::shared_ptr periodicity_statistics; }; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index da7ef04bf2..b8c3feb448 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.29.0 + 6.0.1 The main runnable entrypoint of ros2_control and home of controller management and resource management. Bence Magyar Denis Štogl @@ -32,9 +32,11 @@ std_msgs libstatistics_collector generate_parameter_library + fmt python3-filelock ament_cmake_gmock + ament_cmake_gtest ament_cmake_pytest hardware_interface_testing launch_testing_ros diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3f2898aac8..873b8a95cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,6 +14,8 @@ #include "controller_manager/controller_manager.hpp" +#include + #include #include #include @@ -153,11 +155,23 @@ void controller_chain_spec_cleanup( const auto preceding_controllers = ctrl_chain_spec[controller].preceding_controllers; for (const auto & flwg_ctrl : following_controllers) { - ros2_control::remove_item(ctrl_chain_spec[flwg_ctrl].preceding_controllers, controller); + if (!ros2_control::remove_item(ctrl_chain_spec[flwg_ctrl].preceding_controllers, controller)) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' is not in the list of preceding controllers of '%s'.", controller.c_str(), + flwg_ctrl.c_str()); + } } for (const auto & preced_ctrl : preceding_controllers) { - ros2_control::remove_item(ctrl_chain_spec[preced_ctrl].following_controllers, controller); + if (ros2_control::remove_item(ctrl_chain_spec[preced_ctrl].following_controllers, controller)) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' is not in the list of following controllers of '%s'.", controller.c_str(), + preced_ctrl.c_str()); + } } ctrl_chain_spec.erase(controller); } @@ -280,15 +294,15 @@ controller_interface::return_type evaluate_switch_result( RCLCPP_ERROR(logger, "%s", message.c_str()); if (!unable_to_activate_controllers.empty()) { - const std::string error_msg = - "Unable to activate controllers: [ " + unable_to_activate_controllers + "]"; + const std::string error_msg = fmt::format( + FMT_COMPILE("Unable to activate controllers: [ {} ]"), unable_to_activate_controllers); message += "\n" + error_msg; RCLCPP_ERROR(logger, "%s", error_msg.c_str()); } if (!unable_to_deactivate_controllers.empty()) { - const std::string error_msg = - "Unable to deactivate controllers: [ " + unable_to_deactivate_controllers + "]"; + const std::string error_msg = fmt::format( + FMT_COMPILE("Unable to deactivate controllers: [ {} ]"), unable_to_deactivate_controllers); message += "\n" + error_msg; RCLCPP_ERROR(logger, "%s", error_msg.c_str()); } @@ -303,7 +317,8 @@ controller_interface::return_type evaluate_switch_result( std::string list = std::accumulate( std::next(deactivate_list.begin()), deactivate_list.end(), deactivate_list.front(), [](std::string a, std::string b) { return a + " " + b; }); - const std::string info_msg = "Deactivated controllers: [ " + list + " ]"; + const std::string info_msg = + fmt::format(FMT_COMPILE("Deactivated controllers: [ {} ]"), list); message += "\n" + info_msg; RCLCPP_INFO(logger, "%s", info_msg.c_str()); } @@ -312,7 +327,8 @@ controller_interface::return_type evaluate_switch_result( std::string list = std::accumulate( std::next(activate_list.begin()), activate_list.end(), activate_list.front(), [](std::string a, std::string b) { return a + " " + b; }); - const std::string info_msg = "Activated controllers: [ " + list + " ]"; + const std::string info_msg = + fmt::format(FMT_COMPILE("Activated controllers: [ {} ]"), list); message += "\n" + info_msg; RCLCPP_INFO(logger, "%s", info_msg.c_str()); } @@ -340,6 +356,69 @@ void get_controller_list_command_interfaces( } } } + +void get_full_chain_spec( + const std::string & controller_name, + const std::unordered_map & chain_spec, + std::vector & full_chain_list) +{ + auto it = chain_spec.find(controller_name); + if (it == chain_spec.end()) + { + RCLCPP_WARN( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the controller chain specification.", controller_name.c_str()); + return; + } + ros2_control::add_item(full_chain_list, controller_name); + for (const auto & following_controller : it->second.following_controllers) + { + if (!ros2_control::has_item(full_chain_list, following_controller)) + { + get_full_chain_spec(following_controller, chain_spec, full_chain_list); + } + } + for (const auto & preceding_controller : it->second.preceding_controllers) + { + if (!ros2_control::has_item(full_chain_list, preceding_controller)) + { + get_full_chain_spec(preceding_controller, chain_spec, full_chain_list); + } + } +} + +void build_controller_full_chain_map_cache( + const std::unordered_map & + controller_chain_spec, + std::unordered_map> & controller_full_chain_info_cache) +{ + controller_full_chain_info_cache.clear(); + for (const auto & [controller_name, chain_spec] : controller_chain_spec) + { + controller_full_chain_info_cache[controller_name] = {}; + // add the controller itself to the list + ros2_control::add_item(controller_full_chain_info_cache[controller_name], controller_name); + get_full_chain_spec( + controller_name, controller_chain_spec, controller_full_chain_info_cache[controller_name]); + } +} + +void register_controller_manager_statistics( + const std::string & name, + const libstatistics_collector::moving_average_statistics::StatisticData * variable) +{ + REGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name, variable); +} + +void unregister_controller_manager_statistics(const std::string & name) +{ + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/max"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/min"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/average"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/standard_deviation"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/sample_count"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/current_value"); +} } // namespace namespace controller_manager @@ -360,21 +439,8 @@ rclcpp::NodeOptions get_cm_node_options() ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) -: rclcpp::Node(manager_node_name, node_namespace, options), - diagnostics_updater_(this), - executor_(executor), - loader_( - std::make_shared>( - kControllerInterfaceNamespace, kControllerInterfaceClassName)), - chainable_loader_( - std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), - cm_node_options_(options) +: ControllerManager(executor, "", false, manager_node_name, node_namespace, options) { - initialize_parameters(); - resource_manager_ = - std::make_unique(trigger_clock_, this->get_logger()); - init_controller_manager(); } ControllerManager::ControllerManager( @@ -394,8 +460,20 @@ ControllerManager::ControllerManager( robot_description_(urdf) { initialize_parameters(); - resource_manager_ = std::make_unique( - urdf, trigger_clock_, this->get_logger(), activate_all_hw_components, params_->update_rate); + hardware_interface::ResourceManagerParams params; + params.robot_description = robot_description_; + params.clock = trigger_clock_; + params.logger = this->get_logger(); + params.activate_all = activate_all_hw_components; + params.update_rate = static_cast(params_->update_rate); + params.executor = executor_; + params.node_namespace = node_namespace; + params.allow_controller_activation_with_inactive_hardware = + params_->defaults.allow_controller_activation_with_inactive_hardware; + params.return_failed_hardware_names_on_return_deactivate_write_cycle_ = + params_->defaults.deactivate_controllers_on_hardware_self_deactivate; + resource_manager_ = + std::make_unique(params, !robot_description_.empty()); init_controller_manager(); } @@ -463,34 +541,38 @@ bool ControllerManager::shutdown_controllers() void ControllerManager::init_controller_manager() { - controller_manager_activity_publisher_ = - create_publisher( - "~/activity", rclcpp::QoS(1).reliable().transient_local()); - rt_controllers_wrapper_.set_on_switch_callback( - std::bind(&ControllerManager::publish_activity, this)); - resource_manager_->set_on_component_state_switch_callback( - std::bind(&ControllerManager::publish_activity, this)); + if (!resource_manager_ && !robot_description_.empty()) + { + init_resource_manager(robot_description_); + } - // Get parameters needed for RT "update" loop to work - if (is_resource_manager_initialized()) + if (!is_resource_manager_initialized()) { - if (enforce_command_limits_) + // fallback state + resource_manager_ = std::make_unique(trigger_clock_, get_logger()); + if (!robot_description_notification_timer_) { - resource_manager_->import_joint_limiters(robot_description_); + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN(get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); + }); } - init_services(); - } - else + }else { - robot_description_notification_timer_ = create_wall_timer( - std::chrono::seconds(1), - [&]() - { - RCLCPP_WARN( - get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); - }); + RCLCPP_INFO(get_logger(), + "Resource Manager has been successfully initialized. Starting Controller Manager " + "services..."); + init_services(); } + controller_manager_activity_publisher_ = + create_publisher( + "~/activity", rclcpp::QoS(1).reliable().transient_local()); + rt_controllers_wrapper_.set_on_switch_callback( + std::bind(&ControllerManager::publish_activity, this)); + // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) robot_description_subscription_ = create_subscription( @@ -501,7 +583,7 @@ void ControllerManager::init_controller_manager() robot_description_subscription_->get_topic_name()); // Setup diagnostics - periodicity_stats_.Reset(); + periodicity_stats_.reset(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -516,6 +598,9 @@ void ControllerManager::init_controller_manager() this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, hardware_interface::DEFAULT_REGISTRY_KEY); START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY); + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY( + this, hardware_interface::CM_STATISTICS_TOPIC, hardware_interface::CM_STATISTICS_KEY); + START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::CM_STATISTICS_KEY); // Add on_shutdown callback to stop the controller manager rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context(); @@ -540,12 +625,8 @@ void ControllerManager::init_controller_manager() RCLCPP_INFO(get_logger(), "Shutting down the controller manager."); })); - // Declare the enforce_command_limits parameter such a way that it is enabled by default for - // rolling and newer alone - enforce_command_limits_ = - this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false); RCLCPP_INFO_EXPRESSION( - get_logger(), enforce_command_limits_, "Enforcing command limits is enabled..."); + get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled..."); } void ControllerManager::initialize_parameters() @@ -553,13 +634,25 @@ void ControllerManager::initialize_parameters() // Initialize parameters try { + use_sim_time_ = this->get_parameter("use_sim_time").as_bool(); + + if (!this->has_parameter("overruns.print_warnings")) + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = + "If true, the controller manager will print a warning message to the console if an overrun " + "is detected in its real-time loop (read, update and write). By default, it is set to " + "true, except when used with use_sim_time parameter set to true."; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(!use_sim_time_); + this->declare_parameter("overruns.print_warnings", parameter, descriptor); + } 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); - const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); trigger_clock_ = - use_sim_time.as_bool() ? this->get_clock() : std::make_shared(RCL_STEADY_TIME); + use_sim_time_ ? this->get_clock() : std::make_shared(RCL_STEADY_TIME); RCLCPP_INFO( get_logger(), "Using %s clock for triggering controller manager cycles.", trigger_clock_->get_clock_type() == RCL_STEADY_TIME ? "Steady (Monotonic)" : "ROS"); @@ -585,9 +678,15 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & get_logger(), "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description."); return; - } + } + init_resource_manager(robot_description_); - if (is_resource_manager_initialized()) + if (!is_resource_manager_initialized()) + { + // The RM failed to init AFTER we received the description - a critical error. + // don't finalize controller manager, instead keep waiting for robot description - fallback state + resource_manager_ = std::make_unique(trigger_clock_, get_logger()); + } else { RCLCPP_INFO( get_logger(), @@ -599,19 +698,51 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & void ControllerManager::init_resource_manager(const std::string & robot_description) { - if (enforce_command_limits_) + hardware_interface::ResourceManagerParams params; + params.robot_description = robot_description; + params.clock = trigger_clock_; + params.logger = this->get_logger(); + params.executor = executor_; + params.node_namespace = this->get_namespace(); + params.update_rate = static_cast(params_->update_rate); + resource_manager_ = std::make_unique(params, false); + + RCLCPP_INFO_EXPRESSION( + get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled..."); + if (params_->enforce_command_limits) { - resource_manager_->import_joint_limiters(robot_description_); + try + { + resource_manager_->import_joint_limiters(robot_description); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_logger(), "Error importing joint limiters: %s", e.what()); + return; + } } - if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_)) + + try { - RCLCPP_WARN( + if (!resource_manager_->load_and_initialize_components(params)) + { + RCLCPP_WARN( + get_logger(), + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + return; + } + } + catch (const std::exception &e) + { + // Other possible errors when loading components + RCLCPP_ERROR( get_logger(), - "Could not load and initialize hardware. Please check previous output for more details. " - "After you have corrected your URDF, try to publish robot description again."); + "Exception caught while loading and initializing components: %s", e.what()); return; } - + resource_manager_->set_on_component_state_switch_callback(std::bind(&ControllerManager::publish_activity, this)); + // Get all components and if they are not defined in parameters activate them automatically auto components_to_activate = resource_manager_->get_components_status(); @@ -641,9 +772,19 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript 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()); + if (params_->hardware_components_initial_state.shutdown_on_initial_state_failure) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Failed to set the initial state of the component : {} to {}"), + component.c_str(), state.label())); + } + else + { + RCLCPP_ERROR( + get_logger(), "Failed to set the initial state of the component : '%s' to '%s'", + component.c_str(), state.label().c_str()); + } } components_to_activate.erase(component); } @@ -676,14 +817,80 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript resource_manager_->set_component_state(component, active_state) == hardware_interface::return_type::ERROR) { - throw std::runtime_error( - "Failed to set the initial state of the component : " + component + " to " + - active_state.label()); + if (params_->hardware_components_initial_state.shutdown_on_initial_state_failure) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Failed to set the initial state of the component : {} to {}"), + component.c_str(), active_state.label())); + } + else + { + RCLCPP_ERROR( + get_logger(), "Failed to set the initial state of the component : '%s' to '%s'", + component.c_str(), active_state.label().c_str()); + } } } robot_description_notification_timer_->cancel(); + + auto hw_components_info = resource_manager_->get_components_status(); + + for (const auto & [component_name, component_info] : hw_components_info) + { + if (component_name.empty()) + { + RCLCPP_WARN( + get_logger(), "Component name is empty, skipping statistics registration for it."); + continue; + } + if (!component_info.read_statistics && !component_info.write_statistics) + { + RCLCPP_WARN( + get_logger(), + "Component '%s' does not have read or write statistics initialized, skipping registration.", + component_name.c_str()); + continue; + } + RCLCPP_INFO(get_logger(), "Registering statistics for : %s", component_name.c_str()); + const std::string read_cycle_exec_time_prefix = + component_name + ".stats/read_cycle/execution_time"; + const std::string read_cycle_periodicity_prefix = + component_name + ".stats/read_cycle/periodicity"; + register_controller_manager_statistics( + read_cycle_exec_time_prefix, + &component_info.read_statistics->execution_time.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, read_cycle_exec_time_prefix + "/current_value", + &component_info.read_statistics->execution_time.get_current_data()); + register_controller_manager_statistics( + read_cycle_periodicity_prefix, &component_info.read_statistics->periodicity.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, read_cycle_periodicity_prefix + "/current_value", + &component_info.read_statistics->periodicity.get_current_data()); + if (component_info.write_statistics) + { + const std::string write_cycle_exec_time_prefix = + component_name + ".stats/write_cycle/execution_time"; + const std::string write_cycle_periodicity_prefix = + component_name + ".stats/write_cycle/periodicity"; + register_controller_manager_statistics( + write_cycle_exec_time_prefix, + &component_info.write_statistics->execution_time.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, write_cycle_exec_time_prefix + "/current_value", + &component_info.write_statistics->execution_time.get_current_data()); + register_controller_manager_statistics( + write_cycle_periodicity_prefix, + &component_info.write_statistics->periodicity.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, write_cycle_periodicity_prefix + "/current_value", + &component_info.write_statistics->periodicity.get_current_data()); + } + } } + void ControllerManager::init_services() { // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on @@ -736,6 +943,30 @@ void ControllerManager::init_services() "~/set_hardware_component_state", std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), qos_services, best_effort_callback_group_); + + const std::string cm_name = get_name(); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".update_time", &execution_time_.update_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".read_time", &execution_time_.read_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".write_time", &execution_time_.write_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".total_time", &execution_time_.total_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".switch_time", &execution_time_.switch_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".switch_chained_mode_time", + &execution_time_.switch_chained_mode_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".switch_perform_mode_time", + &execution_time_.switch_perform_mode_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".deactivation_time", + &execution_time_.deactivation_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".activation_time", + &execution_time_.activation_time); } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( @@ -801,12 +1032,26 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c std::make_shared(0, 0, this->get_trigger_clock()->get_clock_type()); controller_spec.execution_time_statistics = std::make_shared(); controller_spec.periodicity_statistics = std::make_shared(); + const std::string controller_exec_time_prefix = controller_name + ".stats/execution_time"; + const std::string controller_periodicity_prefix = controller_name + ".stats/periodicity"; + register_controller_manager_statistics( + controller_exec_time_prefix, + &controller_spec.execution_time_statistics->get_statistics_const_ptr()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, controller_exec_time_prefix + "/current_value", + &controller_spec.execution_time_statistics->get_current_measurement_const_ptr()); + register_controller_manager_statistics( + controller_periodicity_prefix, + &controller_spec.periodicity_statistics->get_statistics_const_ptr()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, controller_periodicity_prefix + "/current_value", + &controller_spec.periodicity_statistics->get_current_measurement_const_ptr()); // 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"; + const std::string param_name = fmt::format(FMT_COMPILE("{}.params_file"), controller_name); controller_spec.info.parameters_files.clear(); // get_parameter checks if parameter has been declared/set @@ -831,7 +1076,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } } - const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; + const std::string fallback_ctrl_param = + fmt::format(FMT_COMPILE("{}.fallback_controllers"), controller_name); std::vector fallback_controllers; if (!has_parameter(fallback_ctrl_param)) { @@ -851,7 +1097,8 @@ 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"; + const std::string node_options_args_param = + fmt::format(FMT_COMPILE("{}.node_options_args"), controller_name); std::vector node_options_args; if (!has_parameter(node_options_args_param)) { @@ -868,7 +1115,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name) { - const std::string param_name = controller_name + ".type"; + const std::string param_name = fmt::format(FMT_COMPILE("{}.type"), controller_name); std::string controller_type; // We cannot declare the parameters for the controllers that will be loaded in the future, @@ -938,6 +1185,8 @@ controller_interface::return_type ControllerManager::unload_controller( get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str()); shutdown_controller(controller); } + unregister_controller_manager_statistics(controller_name + ".stats/execution_time"); + unregister_controller_manager_statistics(controller_name + ".stats/periodicity"); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -1055,6 +1304,8 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } } + // For cases, when the controller ends up in the unconfigured state from any other state + cleanup_controller_exported_interfaces(*found_it); try { @@ -1138,6 +1389,10 @@ controller_interface::return_type ControllerManager::configure_controller( resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); resource_manager_->import_controller_exported_state_interfaces( controller_name, state_interfaces); + // make all the exported interfaces of the controller unavailable + controller->set_chained_mode(false); + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } // let's update the list of following and preceding controllers @@ -1173,33 +1428,10 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - for (const auto & cmd_itf : cmd_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) - { - ros2_control::add_item( - controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); - ros2_control::add_item( - controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name); - ros2_control::add_item( - controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller_name); - } - } - // This is needed when we start exporting the state interfaces from the controllers - for (const auto & state_itf : state_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) - { - ros2_control::add_item( - controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); - ros2_control::add_item( - controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name); - ros2_control::add_item( - controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller_name); - } - } + build_controllers_topology_info(controllers); + + std::unordered_map> controller_full_chain_info_cache; + build_controller_full_chain_map_cache(controller_chain_spec_, controller_full_chain_info_cache); // Now let's reorder the controllers // lock controllers @@ -1234,6 +1466,23 @@ controller_interface::return_type ControllerManager::configure_controller( } } + // Update the controllers chain groups in the ControllerSpec + for (const auto & [ctrl_name, full_chain_info] : controller_full_chain_info_cache) + { + RCLCPP_DEBUG( + get_logger(), "%s", + fmt::format( + "The controller '{}' is in chain with: [{}]", ctrl_name, fmt::join(full_chain_info, ", ")) + .c_str()); + auto controller_it = std::find_if( + new_list.begin(), new_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl_name)); + if (controller_it != new_list.end()) + { + controller_it->controllers_chain_group = full_chain_info; + } + } + to = new_list; RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); for (const auto & ctrl : to) @@ -1252,19 +1501,20 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { switch_params_.do_switch = false; - deactivate_request_.clear(); - activate_request_.clear(); + switch_params_.activate_asap = false; + switch_params_.deactivate_request.clear(); + switch_params_.activate_request.clear(); // Set these interfaces as unavailable when clearing requests to avoid leaving them in available // state without the controller being in active state - for (const auto & controller_name : to_chained_mode_request_) + for (const auto & controller_name : switch_params_.to_chained_mode_request) { resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } - to_chained_mode_request_.clear(); - from_chained_mode_request_.clear(); - activate_command_interface_request_.clear(); - deactivate_command_interface_request_.clear(); + switch_params_.to_chained_mode_request.clear(); + switch_params_.from_chained_mode_request.clear(); + switch_params_.activate_command_interface_request.clear(); + switch_params_.deactivate_command_interface_request.clear(); } controller_interface::return_type ControllerManager::switch_controller( @@ -1294,7 +1544,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // reset the switch param internal variables switch_params_.reset(); - if (!deactivate_request_.empty() || !activate_request_.empty()) + if (!switch_params_.deactivate_request.empty() || !switch_params_.activate_request.empty()) { RCLCPP_FATAL( get_logger(), @@ -1303,7 +1553,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } if ( - !deactivate_command_interface_request_.empty() || !activate_command_interface_request_.empty()) + !switch_params_.deactivate_command_interface_request.empty() || + !switch_params_.activate_command_interface_request.empty()) { RCLCPP_FATAL( get_logger(), @@ -1311,7 +1562,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb( "switch_controller() call. This should never happen."); throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } - if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + if ( + !switch_params_.from_chained_mode_request.empty() || + !switch_params_.to_chained_mode_request.empty()) { RCLCPP_FATAL( get_logger(), @@ -1321,13 +1574,37 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } if (strictness == 0) { - RCLCPP_WARN( + std::string default_strictness = params_->defaults.switch_controller.strictness; + // Convert to uppercase + std::transform( + default_strictness.begin(), default_strictness.end(), default_strictness.begin(), + [](unsigned char c) { return std::toupper(c); }); + RCLCPP_WARN_ONCE( get_logger(), "Controller Manager: to switch controllers you need to specify a " "strictness level of controller_manager_msgs::SwitchController::STRICT " - "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT", + "(%d) or ::BEST_EFFORT (%d). When unspecified, the default is %s", controller_manager_msgs::srv::SwitchController::Request::STRICT, - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT); + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + default_strictness.c_str()); + strictness = params_->defaults.switch_controller.strictness == "strict" + ? controller_manager_msgs::srv::SwitchController::Request::STRICT + : controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + } + else if (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO) + { + RCLCPP_WARN( + get_logger(), + "Controller Manager: AUTO is not currently implemented. " + "Defaulting to BEST_EFFORT"); + strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + } + else if (strictness == controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO) + { + RCLCPP_DEBUG( + get_logger(), + "Controller Manager: FORCE_AUTO is not currently implemented. " + "Defaulting to BEST_EFFORT"); strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } @@ -1370,8 +1647,10 @@ controller_interface::return_type ControllerManager::switch_controller_cb( if (found_it == updated_controllers.end()) { - const std::string error_msg = "Could not " + action + " controller with name '" + - controller + "' because no controller with this name exists"; + const std::string error_msg = fmt::format( + FMT_COMPILE( + "Could not {} controller with name '{}' because no controller with this name exists"), + action, controller); msg += error_msg + "\n"; RCLCPP_WARN(get_logger(), "%s", error_msg.c_str()); // For the BEST_EFFORT switch, if there are more controllers that are in the list, this is @@ -1395,25 +1674,28 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } } RCLCPP_DEBUG( - get_logger(), "'%s' request vector has size %i", action.c_str(), (int)request_list.size()); + get_logger(), "'%s' request vector has size %i", action.c_str(), + static_cast(request_list.size())); return result; }; // list all controllers to deactivate (check if all controllers exist) - auto ret = list_controllers(deactivate_controllers, deactivate_request_, "deactivate", message); + auto ret = list_controllers( + deactivate_controllers, switch_params_.deactivate_request, "deactivate", message); if (ret != controller_interface::return_type::OK) { - deactivate_request_.clear(); + switch_params_.deactivate_request.clear(); return ret; } // list all controllers to activate (check if all controllers exist) - ret = list_controllers(activate_controllers, activate_request_, "activate", message); + ret = + list_controllers(activate_controllers, switch_params_.activate_request, "activate", message); if (ret != controller_interface::return_type::OK) { - deactivate_request_.clear(); - activate_request_.clear(); + switch_params_.deactivate_request.clear(); + switch_params_.activate_request.clear(); return ret; } // If it is a best effort switch, we can remove the controllers log that could not be activated @@ -1429,7 +1711,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( propagate_deactivation_of_chained_mode(controllers); // check if controllers should be switched 'to' chained mode when controllers are activated - for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + for (auto ctrl_it = switch_params_.activate_request.begin(); + ctrl_it != switch_params_.activate_request.end(); ++ctrl_it) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1439,9 +1722,11 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // if controller is not inactive then do not do any following-controllers checks if (is_controller_unconfigured(*controller_it->c)) { - message = "Controller with name '" + controller_it->info.name + - "' is in 'unconfigured' state. The controller needs to be configured to be in " - "'inactive' state before it can be checked and activated."; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' is in 'unconfigured' state. The controller needs to be " + "configured to be in 'inactive' state before it can be checked and activated."), + controller_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } @@ -1449,19 +1734,23 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { if ( std::find( - deactivate_request_.begin(), deactivate_request_.end(), controller_it->info.name) == - deactivate_request_.end()) + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller_it->info.name) == switch_params_.deactivate_request.end()) { - message = "Controller with name '" + controller_it->info.name + "' is already active."; + message = fmt::format( + FMT_COMPILE("Controller with name '{}' is already active."), controller_it->info.name); + RCLCPP_WARN(get_logger(), "%s", message.c_str()); RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } } else if (!is_controller_inactive(controller_it->c)) { - message = "Controller with name '" + controller_it->info.name + - "' is not in 'inactive' state. The controller needs to be in 'inactive' state " - "before it can be checked and activated."; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' is not in 'inactive' state. The controller needs to be in " + "'inactive' state before it can be checked and activated."), + controller_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } @@ -1491,7 +1780,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop - activate_request_.erase(ctrl_it); + switch_params_.activate_request.erase(ctrl_it); message.clear(); --ctrl_it; } @@ -1506,7 +1795,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } // check if controllers should be deactivated if used in chained mode - for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it) + for (auto ctrl_it = switch_params_.deactivate_request.begin(); + ctrl_it != switch_params_.deactivate_request.end(); ++ctrl_it) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1516,8 +1806,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // if controller is not active then skip preceding-controllers checks if (!is_controller_active(controller_it->c)) { - message = "Controller with name '" + controller_it->info.name + - "' can not be deactivated since it is not active."; + message = fmt::format( + FMT_COMPILE("Controller with name '{}' can not be deactivated since it is not active."), + controller_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } @@ -1539,7 +1830,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop - deactivate_request_.erase(ctrl_it); + switch_params_.deactivate_request.erase(ctrl_it); message.clear(); --ctrl_it; } @@ -1554,7 +1845,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } // Check after the check if the activate and deactivate list is empty or not - if (activate_request_.empty() && deactivate_request_.empty()) + if (switch_params_.activate_request.empty() && switch_params_.deactivate_request.empty()) { message = "After checking the controllers, no controllers need to be activated or deactivated."; RCLCPP_INFO(get_logger(), "%s", message.c_str()); @@ -1566,16 +1857,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb( for (const auto & controller : controllers) { auto to_chained_mode_list_it = std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), controller.info.name); - bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end(); + switch_params_.to_chained_mode_request.begin(), switch_params_.to_chained_mode_request.end(), + controller.info.name); + bool in_to_chained_mode_list = + to_chained_mode_list_it != switch_params_.to_chained_mode_request.end(); auto from_chained_mode_list_it = std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); - bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); + switch_params_.from_chained_mode_request.begin(), + switch_params_.from_chained_mode_request.end(), controller.info.name); + bool in_from_chained_mode_list = + from_chained_mode_list_it != switch_params_.from_chained_mode_request.end(); - auto deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(); + auto deactivate_list_it = std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller.info.name); + bool in_deactivate_list = deactivate_list_it != switch_params_.deactivate_request.end(); const bool is_active = is_controller_active(*controller.c); const bool is_inactive = is_controller_inactive(*controller.c); @@ -1585,19 +1881,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { if (is_active && !in_deactivate_list) { - deactivate_request_.push_back(controller.info.name); - activate_request_.push_back(controller.info.name); + switch_params_.deactivate_request.push_back(controller.info.name); + switch_params_.activate_request.push_back(controller.info.name); } } // get pointers to places in deactivate and activate lists ((de)activate lists have changed) - deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - in_deactivate_list = deactivate_list_it != deactivate_request_.end(); + deactivate_list_it = std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller.info.name); + in_deactivate_list = deactivate_list_it != switch_params_.deactivate_request.end(); - auto activate_list_it = - std::find(activate_request_.begin(), activate_request_.end(), controller.info.name); - bool in_activate_list = activate_list_it != activate_request_.end(); + auto activate_list_it = std::find( + switch_params_.activate_request.begin(), switch_params_.activate_request.end(), + controller.info.name); + bool in_activate_list = activate_list_it != switch_params_.activate_request.end(); auto handle_conflict = [&](const std::string & msg) { @@ -1605,12 +1903,12 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { message = msg; RCLCPP_ERROR(get_logger(), "%s", msg.c_str()); - deactivate_request_.clear(); - deactivate_command_interface_request_.clear(); - activate_request_.clear(); - activate_command_interface_request_.clear(); - to_chained_mode_request_.clear(); - from_chained_mode_request_.clear(); + switch_params_.deactivate_request.clear(); + switch_params_.deactivate_command_interface_request.clear(); + switch_params_.activate_request.clear(); + switch_params_.activate_command_interface_request.clear(); + switch_params_.to_chained_mode_request.clear(); + switch_params_.from_chained_mode_request.clear(); return controller_interface::return_type::ERROR; } RCLCPP_WARN(get_logger(), "%s", msg.c_str()); @@ -1627,7 +1925,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( return conflict_status; } in_deactivate_list = false; - deactivate_request_.erase(deactivate_list_it); + switch_params_.deactivate_request.erase(deactivate_list_it); } // check for doubled activation @@ -1640,7 +1938,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( return conflict_status; } in_activate_list = false; - activate_request_.erase(activate_list_it); + switch_params_.activate_request.erase(activate_list_it); } // check for illegal activation of an unconfigured/finalized controller @@ -1654,18 +1952,18 @@ controller_interface::return_type ControllerManager::switch_controller_cb( return conflict_status; } in_activate_list = false; - activate_request_.erase(activate_list_it); + switch_params_.activate_request.erase(activate_list_it); } if (in_activate_list) { extract_command_interfaces_for_controller( - controller, resource_manager_, activate_command_interface_request_); + controller, resource_manager_, switch_params_.activate_command_interface_request); } if (in_deactivate_list) { extract_command_interfaces_for_controller( - controller, resource_manager_, deactivate_command_interface_request_); + controller, resource_manager_, switch_params_.deactivate_command_interface_request); } // cache mapping between hardware and controllers for stopping when read/write error happens @@ -1708,7 +2006,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } } - if (activate_request_.empty() && deactivate_request_.empty()) + if (switch_params_.activate_request.empty() && switch_params_.deactivate_request.empty()) { message = "After checking the controllers, no controllers need to be activated or deactivated."; RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); @@ -1717,7 +2015,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } if ( - check_for_interfaces_availability_to_activate(controllers, activate_request_, message) != + check_for_interfaces_availability_to_activate( + controllers, switch_params_.activate_request, switch_params_.deactivate_request, message) != controller_interface::return_type::OK) { clear_requests(); @@ -1725,18 +2024,18 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); - for (const auto & interface : activate_command_interface_request_) + for (const auto & interface : switch_params_.activate_command_interface_request) { RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } RCLCPP_DEBUG(get_logger(), "Release of command interfaces from deactivating controllers:"); - for (const auto & interface : deactivate_command_interface_request_) + for (const auto & interface : switch_params_.deactivate_command_interface_request) { RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } // wait for deactivating async controllers to finish their current cycle - for (const auto & controller : deactivate_request_) + for (const auto & controller : switch_params_.deactivate_request) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1748,10 +2047,12 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } if ( - !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) + !switch_params_.activate_command_interface_request.empty() || + !switch_params_.deactivate_command_interface_request.empty()) { if (!resource_manager_->prepare_command_mode_switch( - activate_command_interface_request_, deactivate_command_interface_request_)) + switch_params_.activate_command_interface_request, + switch_params_.deactivate_command_interface_request)) { message = "Could not switch controllers since prepare command mode switch was rejected."; RCLCPP_ERROR(get_logger(), "%s", message.c_str()); @@ -1774,17 +2075,27 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } switch_params_.do_switch = true; // wait until switch is finished - RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - std::unique_lock switch_params_guard(switch_params_.mutex, std::defer_lock); - if (!switch_params_.cv.wait_for( - switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) - { - message = "Switch controller timed out after " + - std::to_string(static_cast(switch_params_.timeout.count()) / 1e9) + - " seconds!"; - RCLCPP_ERROR(get_logger(), "%s", message.c_str()); - clear_requests(); - return controller_interface::return_type::ERROR; + if (switch_params_.activate_asap) + { + RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); + std::unique_lock switch_params_guard(switch_params_.mutex); + if (!switch_params_.cv.wait_for( + switch_params_guard, switch_params_.timeout, + [this] { return !switch_params_.do_switch; })) + { + message = fmt::format( + FMT_COMPILE("Switch controller timed out after {} seconds!"), + static_cast(switch_params_.timeout.count()) / 1e9); + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); + clear_requests(); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_INFO(get_logger(), "Requested controller switch from non-realtime loop"); + // This should work as the realtime thread operation is read-only operation + manage_switch(); } // copy the controllers spec from the used to the unused list @@ -1793,8 +2104,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // update the claimed interface controller info auto switch_result = evaluate_switch_result( - resource_manager_, activate_request_, deactivate_request_, strictness, get_logger(), to, - message); + resource_manager_, switch_params_.activate_request, switch_params_.deactivate_request, + strictness, get_logger(), to, message); // switch lists rt_controllers_wrapper_.switch_updated_list(guard); @@ -1835,10 +2146,15 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co // Catch whatever exception the controller might throw try { - if ( - controller.c->init( - controller.info.name, robot_description_, get_update_rate(), get_namespace(), - controller_node_options) == controller_interface::return_type::ERROR) + controller_interface::ControllerInterfaceParams controller_params; + controller_params.controller_name = controller.info.name; + controller_params.robot_description = robot_description_; + controller_params.update_rate = get_update_rate(); + controller_params.node_namespace = get_namespace(); + controller_params.node_options = controller_node_options; + controller_params.hard_joint_limits = resource_manager_->get_hard_joint_limits(); + controller_params.soft_joint_limits = resource_manager_->get_soft_joint_limits(); + if (controller.c->init(controller_params) == controller_interface::return_type::ERROR) { to.clear(); RCLCPP_ERROR( @@ -1886,7 +2202,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co void ControllerManager::deactivate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_deactivate) + const std::vector & controllers_to_deactivate) { // deactivate controllers for (const auto & controller_name : controllers_to_deactivate) @@ -1914,6 +2230,7 @@ void ControllerManager::deactivate_controllers( // deactivation if (controller->is_chainable()) { + controller->set_chained_mode(false); resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } @@ -1990,8 +2307,10 @@ void ControllerManager::switch_chained_mode( void ControllerManager::activate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_activate) + const std::vector & controllers_to_activate, int strictness) { + std::vector failed_controllers_command_interfaces; + bool is_successful = true; for (const auto & controller_name : controllers_to_activate) { auto found_it = std::find_if( @@ -2058,6 +2377,7 @@ void ControllerManager::activate_controllers( // something went wrong during command interfaces, go skip the controller if (!assignment_successful) { + is_successful = false; continue; } @@ -2096,39 +2416,48 @@ void ControllerManager::activate_controllers( // something went wrong during state interfaces, go skip the controller if (!assignment_successful) { + is_successful = false; continue; } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); + auto new_state = controller->get_lifecycle_state(); try { - found_it->periodicity_statistics->Reset(); - found_it->execution_time_statistics->Reset(); - const auto new_state = controller->get_node()->activate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - RCLCPP_ERROR( - get_logger(), - "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", - controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), - hardware_interface::lifecycle_state_names::ACTIVE, - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - } + found_it->periodicity_statistics->reset(); + found_it->execution_time_statistics->reset(); + new_state = controller->get_node()->activate(); } catch (const std::exception & e) { RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); - controller->release_interfaces(); - continue; } catch (...) { RCLCPP_ERROR( get_logger(), "Caught unknown exception while activating the controller '%s'", controller_name.c_str()); + } + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR( + get_logger(), + "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d). Releasing " + "interfaces!", + controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), + hardware_interface::lifecycle_state_names::ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + is_successful = false; controller->release_interfaces(); + ros2_control::add_items(failed_controllers_command_interfaces, command_interface_names); + if (controller->is_chainable()) + { + // make all the exported interfaces of the controller unavailable + resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } continue; } @@ -2140,14 +2469,39 @@ void ControllerManager::activate_controllers( resource_manager_->make_controller_reference_interfaces_available(controller_name); } } -} - -void ControllerManager::activate_controllers_asap( - const std::vector & rt_controller_list, - const std::vector controllers_to_activate) -{ - // https://github.com/ros-controls/ros2_control/issues/263 - activate_controllers(rt_controller_list, controllers_to_activate); + if ( + !is_successful && strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR( + get_logger(), + "At least one controller failed to be activated. Releasing all interfaces and stopping all " + "activated controllers because the switch strictness is set to STRICT."); + // deactivate all controllers that were activated in this switch + deactivate_controllers(rt_controller_list, controllers_to_activate); + if ( + !resource_manager_->prepare_command_mode_switch( + {}, switch_params_.activate_command_interface_request) || + !resource_manager_->perform_command_mode_switch( + {}, switch_params_.activate_command_interface_request)) + { + RCLCPP_ERROR( + get_logger(), + "Error switching back the interfaces in the hardware when the controller activation " + "failed."); + } + } + // Now prepare and perform the stop interface switching as this is needed for exclusive + // interfaces + else if ( + !failed_controllers_command_interfaces.empty() && + (!resource_manager_->prepare_command_mode_switch({}, failed_controllers_command_interfaces) || + !resource_manager_->perform_command_mode_switch({}, failed_controllers_command_interfaces))) + { + RCLCPP_ERROR( + get_logger(), + "Error switching back the interfaces in the hardware when the controller activation " + "failed."); + } } void ControllerManager::list_controllers_srv_cb( @@ -2361,7 +2715,7 @@ void ControllerManager::reload_controller_libraries_service_cb( get_logger(), "Controller manager: Cannot reload controller libraries because" " there are still %i active controllers", - (int)active_controllers.size()); + static_cast(active_controllers.size())); response->ok = false; return; } @@ -2477,6 +2831,7 @@ void ControllerManager::list_hardware_components_srv_cb( { controller_manager_msgs::msg::HardwareInterface hwi; hwi.name = interface; + hwi.data_type = resource_manager_->get_command_interface_data_type(interface); hwi.is_available = resource_manager_->command_interface_is_available(interface); hwi.is_claimed = resource_manager_->command_interface_is_claimed(interface); // TODO(destogl): Add here mapping to controller that has claimed or @@ -2501,6 +2856,7 @@ void ControllerManager::list_hardware_components_srv_cb( { controller_manager_msgs::msg::HardwareInterface hwi; hwi.name = interface; + hwi.data_type = resource_manager_->get_state_interface_data_type(interface); hwi.is_available = resource_manager_->state_interface_is_available(interface); hwi.is_claimed = false; component.state_interfaces.push_back(hwi); @@ -2526,6 +2882,7 @@ void ControllerManager::list_hardware_interfaces_srv_cb( controller_manager_msgs::msg::HardwareInterface hwi; hwi.name = state_interface_name; hwi.is_available = resource_manager_->state_interface_is_available(state_interface_name); + hwi.data_type = resource_manager_->get_state_interface_data_type(state_interface_name); hwi.is_claimed = false; response->state_interfaces.push_back(hwi); } @@ -2536,6 +2893,7 @@ void ControllerManager::list_hardware_interfaces_srv_cb( hwi.name = command_interface_name; hwi.is_available = resource_manager_->command_interface_is_available(command_interface_name); hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name); + hwi.data_type = resource_manager_->get_command_interface_data_type(command_interface_name); response->command_interfaces.push_back(hwi); } @@ -2591,10 +2949,11 @@ 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); + periodicity_stats_.add_measurement(1.0 / period.seconds()); + const auto start_time = std::chrono::steady_clock::now(); + auto [result, failed_hardware_names] = resource_manager_->read(time, period); - if (!ok) + if (result != hardware_interface::return_type::OK) { rt_buffer_.deactivate_controllers_list.clear(); // Determine controllers to stop @@ -2620,6 +2979,9 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); // TODO(destogl): do auto-start of broadcasters } + execution_time_.read_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); } void ControllerManager::manage_switch() @@ -2630,40 +2992,65 @@ void ControllerManager::manage_switch() RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); return; } + const auto start_time = std::chrono::steady_clock::now(); // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( - activate_command_interface_request_, deactivate_command_interface_request_)) + switch_params_.activate_command_interface_request, + switch_params_.deactivate_command_interface_request)) { RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); + // If the hardware switching fails, there is no point in continuing to switch controllers + return; } + execution_time_.switch_perform_mode_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - deactivate_controllers(rt_controller_list, deactivate_request_); + const auto deact_start_time = std::chrono::steady_clock::now(); + deactivate_controllers(rt_controller_list, switch_params_.deactivate_request); + execution_time_.deactivation_time = + std::chrono::duration(std::chrono::steady_clock::now() - deact_start_time) + .count(); - switch_chained_mode(to_chained_mode_request_, true); - switch_chained_mode(from_chained_mode_request_, false); + const auto chain_start_time = std::chrono::steady_clock::now(); + switch_chained_mode(switch_params_.to_chained_mode_request, true); + switch_chained_mode(switch_params_.from_chained_mode_request, false); + RCLCPP_DEBUG( + get_logger(), + "Switching %lu controllers to chained mode and %lu controllers from chained mode", + switch_params_.to_chained_mode_request.size(), switch_params_.from_chained_mode_request.size()); + execution_time_.switch_chained_mode_time = + std::chrono::duration(std::chrono::steady_clock::now() - chain_start_time) + .count(); // activate controllers once the switch is fully complete - if (!switch_params_.activate_asap) - { - activate_controllers(rt_controller_list, activate_request_); - } - else - { - // activate controllers as soon as their required joints are done switching - activate_controllers_asap(rt_controller_list, activate_request_); - } + const auto act_start_time = std::chrono::steady_clock::now(); + activate_controllers( + rt_controller_list, switch_params_.activate_request, switch_params_.strictness); + execution_time_.activation_time = + std::chrono::duration(std::chrono::steady_clock::now() - act_start_time) + .count(); // All controllers switched --> switching done switch_params_.do_switch = false; switch_params_.cv.notify_all(); + execution_time_.switch_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); } controller_interface::return_type ControllerManager::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + const auto start_time = std::chrono::steady_clock::now(); + execution_time_.switch_time = 0.0; + execution_time_.switch_chained_mode_time = 0.0; + execution_time_.activation_time = 0.0; + execution_time_.deactivation_time = 0.0; + execution_time_.switch_perform_mode_time = 0.0; std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); @@ -2693,6 +3080,15 @@ controller_interface::return_type ControllerManager::update( rt_buffer_.deactivate_controllers_list.clear(); for (const auto & loaded_controller : rt_controller_list) { + if ( + switch_params_.do_switch && !switch_params_.activate_asap && + switch_params_.skip_cycle(loaded_controller)) + { + RCLCPP_DEBUG( + get_logger(), "Skipping update for controller '%s' as it is being switched", + loaded_controller.info.name.c_str()); + continue; + } // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 if (is_controller_active(*loaded_controller.c)) @@ -2700,8 +3096,8 @@ controller_interface::return_type ControllerManager::update( if ( switch_params_.do_switch && loaded_controller.c->is_async() && std::find( - deactivate_request_.begin(), deactivate_request_.end(), loaded_controller.info.name) != - deactivate_request_.end()) + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + loaded_controller.info.name) != switch_params_.deactivate_request.end()) { RCLCPP_DEBUG( get_logger(), "Skipping update for async controller '%s' as it is being deactivated", @@ -2714,32 +3110,24 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - bool first_update_cycle = false; + const bool first_update_cycle = + (*loaded_controller.last_update_cycle_time == + rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())); const rclcpp::Time current_time = get_clock()->started() ? get_trigger_clock()->now() : time; - if ( - *loaded_controller.last_update_cycle_time == - rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) - { - // last_update_cycle_time is zero after activation - first_update_cycle = true; - *loaded_controller.last_update_cycle_time = current_time; - RCLCPP_DEBUG( - get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", - loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); - } const auto controller_actual_period = - (current_time - *loaded_controller.last_update_cycle_time); - - /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the - /// jitter in the system sleep cycles. - // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have - // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we - // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue - // to keep up with the controller update rate (see issue #1769). + first_update_cycle ? controller_period + : (current_time - *loaded_controller.last_update_cycle_time); + + const double error_now = + std::abs((controller_actual_period.seconds() * controller_update_rate) - 1.0); + const double error_if_skipped = std::abs( + ((controller_actual_period.seconds() + (1.0 / static_cast(update_rate_))) * + controller_update_rate) - + 1.0); const bool controller_go = run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) || - (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; + (error_now <= error_if_skipped) || first_update_cycle; RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2759,12 +3147,12 @@ controller_interface::return_type ControllerManager::update( controller_ret = trigger_result.result; if (trigger_status && trigger_result.execution_time.has_value()) { - loaded_controller.execution_time_statistics->AddMeasurement( + loaded_controller.execution_time_statistics->add_measurement( 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( + loaded_controller.periodicity_statistics->add_measurement( 1.0 / trigger_result.period.value().seconds()); } } @@ -2787,7 +3175,16 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { - rt_buffer_.deactivate_controllers_list.push_back(loaded_controller.info.name); + const std::vector & controller_chain = + loaded_controller.controllers_chain_group; + RCLCPP_INFO_EXPRESSION( + get_logger(), controller_chain.size() > 1, + "Controller '%s' is part of a chain of %lu controllers that will be deactivated.", + loaded_controller.info.name.c_str(), controller_chain.size()); + for (const auto & chained_controller : controller_chain) + { + ros2_control::add_item(rt_buffer_.deactivate_controllers_list, chained_controller); + } ret = controller_ret; } } @@ -2841,7 +3238,9 @@ controller_interface::return_type ControllerManager::update( deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); if (!rt_buffer_.fallback_controllers_list.empty()) { - activate_controllers(rt_controller_list, rt_buffer_.fallback_controllers_list); + activate_controllers( + rt_controller_list, rt_buffer_.fallback_controllers_list, + controller_manager_msgs::srv::SwitchController::Request::STRICT); } // To publish the activity of the failing controllers and the fallback controllers publish_activity(); @@ -2849,21 +3248,27 @@ controller_interface::return_type ControllerManager::update( resource_manager_->enforce_command_limits(period); // there are controllers to (de)activate - if (switch_params_.do_switch) + if (switch_params_.do_switch && switch_params_.activate_asap) { manage_switch(); } PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::DEFAULT_REGISTRY_KEY); + PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::CM_STATISTICS_KEY); + + execution_time_.update_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); return ret; } void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - auto [ok, failed_hardware_names] = resource_manager_->write(time, period); + const auto start_time = std::chrono::steady_clock::now(); + auto [result, failed_hardware_names] = resource_manager_->write(time, period); - if (!ok) + if (result == hardware_interface::return_type::ERROR) { rt_buffer_.deactivate_controllers_list.clear(); // Determine controllers to stop @@ -2891,6 +3296,84 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); // TODO(destogl): do auto-start of broadcasters } + else if (result == hardware_interface::return_type::DEACTIVATE) + { + rt_buffer_.deactivate_controllers_list.clear(); + auto loaded_controllers = get_loaded_controllers(); + // Only stop controllers with active command interfaces to the failed_hardware_names + for (const auto & hardware_name : failed_hardware_names) + { + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + for (const auto & controller : controllers) + { + auto controller_spec = std::find_if( + loaded_controllers.begin(), loaded_controllers.end(), + [&](const controller_manager::ControllerSpec & spec) + { return spec.c->get_name() == controller; }); + if (controller_spec == loaded_controllers.end()) + { + RCLCPP_WARN( + get_logger(), + "Deactivate failed to find controller [%s] in loaded controllers. " + "This can happen due to multiple returns of 'DEACTIVATE' from [%s] write()", + controller.c_str(), hardware_name.c_str()); + continue; + } + std::vector command_interface_names; + extract_command_interfaces_for_controller( + *controller_spec, resource_manager_, command_interface_names); + // if this controller has command interfaces add it to the deactivate_controllers_list + if (!command_interface_names.empty()) + { + rt_buffer_.deactivate_controllers_list.push_back(controller); + } + } + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !rt_buffer_.deactivate_controllers_list.empty(), + "Deactivating controllers [%s] as their command interfaces are tied to DEACTIVATEing " + "hardware components", + rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str()); + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + perform_hardware_command_mode_change( + rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "write"); + deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); + } + execution_time_.write_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); + execution_time_.total_time = + execution_time_.write_time + execution_time_.update_time + execution_time_.read_time; + const double expected_cycle_time = 1.e6 / static_cast(get_update_rate()); + if (params_->overruns.print_warnings && execution_time_.total_time > expected_cycle_time) + { + if (execution_time_.switch_time > 0.0) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, " + "Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform " + "mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), " + "Write " + "time : %.3f us", + execution_time_.total_time, expected_cycle_time, execution_time_.read_time, + execution_time_.update_time, execution_time_.switch_time, + execution_time_.switch_chained_mode_time, execution_time_.switch_perform_mode_time, + execution_time_.activation_time, execution_time_.deactivation_time, + execution_time_.write_time); + } + else + { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, " + "Update time : %.3f us, Write time : %.3f us", + execution_time_.total_time, expected_cycle_time, execution_time_.read_time, + execution_time_.update_time, execution_time_.write_time); + } + } } std::vector & @@ -3016,10 +3499,11 @@ void ControllerManager::propagate_deactivation_of_chained_mode( for (const auto & controller : controllers) { // get pointers to places in deactivate and activate lists ((de)activate lists have changed) - auto deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); + auto deactivate_list_it = std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller.info.name); - if (deactivate_list_it != deactivate_request_.end()) + if (deactivate_list_it != switch_params_.deactivate_request.end()) { // if controller is not active then skip adding following-controllers to "from" chained mode // request @@ -3049,10 +3533,11 @@ void ControllerManager::propagate_deactivation_of_chained_mode( // with matching interface name to "from" chained mode list (if not already in it) if ( std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name) == from_chained_mode_request_.end()) + switch_params_.from_chained_mode_request.begin(), + switch_params_.from_chained_mode_request.end(), + following_ctrl_it->info.name) == switch_params_.from_chained_mode_request.end()) { - from_chained_mode_request_.push_back(following_ctrl_it->info.name); + switch_params_.from_chained_mode_request.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'from chained mode' request.", following_ctrl_it->info.name.c_str()); @@ -3104,10 +3589,11 @@ controller_interface::return_type ControllerManager::check_following_controllers // check if following controller is chainable if (!following_ctrl_it->c->is_chainable()) { - message = "No state/reference interface from controller : '" + ctrl_itf_name + - "' exist, since the following " - "controller with name '" + - following_ctrl_it->info.name + "' is not chainable."; + message = fmt::format( + FMT_COMPILE( + "No state/reference interface from controller : '{}' exist, since the following " + "controller with name '{}' is not chainable."), + ctrl_itf_name, following_ctrl_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3117,22 +3603,29 @@ controller_interface::return_type ControllerManager::check_following_controllers // will following controller be deactivated? if ( std::find( - deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != - deactivate_request_.end()) + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + following_ctrl_it->info.name) != switch_params_.deactivate_request.end()) { - message = "The following controller with name '" + following_ctrl_it->info.name + - "' is currently active but it is requested to be deactivated."; + message = fmt::format( + FMT_COMPILE( + "The following controller with name '{}' is currently active but it is requested to " + "be deactivated."), + following_ctrl_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } // check if following controller will not be activated else if ( - std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) == - activate_request_.end()) + std::find( + switch_params_.activate_request.begin(), switch_params_.activate_request.end(), + following_ctrl_it->info.name) == switch_params_.activate_request.end()) { - message = "The following controller with name '" + following_ctrl_it->info.name + - "' is currently inactive and it is not requested to be activated."; + message = fmt::format( + FMT_COMPILE( + "The following controller with name '{}' is currently inactive and it is not requested " + "to be activated."), + following_ctrl_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3152,14 +3645,15 @@ controller_interface::return_type ControllerManager::check_following_controllers // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) // { // // insert to the begin of activate request list to be activated before preceding controller - // activate_request_.insert(activate_request_.begin(), following_ctrl_name); + // switch_params_.activate_request.insert(switch_params_.activate_request.begin(), + // following_ctrl_name); // } if (!following_ctrl_it->c->is_in_chained_mode()) { auto found_it = std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), - following_ctrl_it->info.name); - if (found_it == to_chained_mode_request_.end()) + switch_params_.to_chained_mode_request.begin(), + switch_params_.to_chained_mode_request.end(), following_ctrl_it->info.name); + if (found_it == switch_params_.to_chained_mode_request.end()) { // if it is a chainable controller, make the reference interfaces available on preactivation // (This is needed when you activate a couple of chainable controller altogether) @@ -3173,7 +3667,7 @@ controller_interface::return_type ControllerManager::check_following_controllers { resource_manager_->make_controller_reference_interfaces_available( following_ctrl_it->info.name); - to_chained_mode_request_.push_back(following_ctrl_it->info.name); + switch_params_.to_chained_mode_request.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'to chained mode' request.", following_ctrl_it->info.name.c_str()); @@ -3184,11 +3678,11 @@ controller_interface::return_type ControllerManager::check_following_controllers { // Check if following controller is in 'from' chained mode list and remove it, if so auto found_it = std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name); - if (found_it != from_chained_mode_request_.end()) + switch_params_.from_chained_mode_request.begin(), + switch_params_.from_chained_mode_request.end(), following_ctrl_it->info.name); + if (found_it != switch_params_.from_chained_mode_request.end()) { - from_chained_mode_request_.erase(found_it); + switch_params_.from_chained_mode_request.erase(found_it); RCLCPP_DEBUG( get_logger(), "Removing controller '%s' in 'from chained mode' request because it " @@ -3232,23 +3726,29 @@ controller_interface::return_type ControllerManager::check_preceding_controllers { if ( is_controller_inactive(found_it->c) && - std::find(activate_request_.begin(), activate_request_.end(), preceding_controller) != - activate_request_.end()) + std::find( + switch_params_.activate_request.begin(), switch_params_.activate_request.end(), + preceding_controller) != switch_params_.activate_request.end()) { - message = "Unable to deactivate controller with name '" + controller_it->info.name + - "' because preceding controller with name '" + preceding_controller + - "' is inactive and will be activated."; + message = fmt::format( + FMT_COMPILE( + "Unable to deactivate controller with name '{}' because preceding controller with " + "name '{}' is inactive and will be activated."), + controller_it->info.name, preceding_controller); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } if ( is_controller_active(found_it->c) && - std::find(deactivate_request_.begin(), deactivate_request_.end(), preceding_controller) == - deactivate_request_.end()) + std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + preceding_controller) == switch_params_.deactivate_request.end()) { - message = "Unable to deactivate controller with name '" + controller_it->info.name + - "' because preceding controller with name '" + preceding_controller + - "' is currently active and will not be deactivated."; + message = fmt::format( + FMT_COMPILE( + "Unable to deactivate controller with name '{}' because preceding controller with " + "name '{}' is currently active and will not be deactivated."), + controller_it->info.name, preceding_controller); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3262,7 +3762,8 @@ controller_interface::return_type ControllerManager::check_preceding_controllers // { // // insert to the begin of activate request list to be activated before preceding // controller - // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); + // switch_params_.activate_request.insert(switch_params_.activate_request.begin(), + // preceding_ctrl_name); // } return controller_interface::return_type::OK; @@ -3280,8 +3781,11 @@ ControllerManager::check_fallback_controllers_state_pre_activation( std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); if (fb_ctrl_it == controllers.end()) { - message = "Unable to find the fallback controller : '" + fb_ctrl + "' of the controller : '" + - controller_it->info.name + "' within the controller list"; + message = fmt::format( + FMT_COMPILE( + "Unable to find the fallback controller : '{}' of the controller : '{}' within the " + "controller list"), + fb_ctrl, controller_it->info.name); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3289,9 +3793,11 @@ ControllerManager::check_fallback_controllers_state_pre_activation( { if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) { - message = "Controller with name '" + controller_it->info.name + - "' cannot be activated, as its fallback controller : '" + fb_ctrl + - "' need to be configured and be in inactive/active state!"; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as its fallback controller : '{}' need " + "to be configured and be in inactive/active state!"), + controller_it->info.name, fb_ctrl); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3321,24 +3827,25 @@ ControllerManager::check_fallback_controllers_state_pre_activation( std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == exported_ref_itfs.end()) { - message = "Controller with name '" + controller_it->info.name + - "' cannot be activated, as the command interface : '" + fb_cmd_itf + - "' required by its fallback controller : '" + fb_ctrl + - "' is not exported by the controller : '" + - following_ctrl_it->info.name + "' in the current fallback list!"; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as the command interface : " + "'{}' required by its fallback controller : '{}' is not exported by the " + "controller : '{}' in the current fallback list!"), + controller_it->info.name, fb_cmd_itf, fb_ctrl, following_ctrl_it->info.name); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } else { - message = - "Controller with name '" + controller_it->info.name + - "' cannot be activated, as the command interface : '" + fb_cmd_itf + - "' required by its fallback controller : '" + fb_ctrl + - "' is not available as the controller is not in active state!. May be " - "consider adding this controller to the fallback list of the controller : '" + - following_ctrl_it->info.name + "' or already have it activated."; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as the command interface : " + "'{}' required by its fallback controller : '{}' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '{}' or already have it activated."), + controller_it->info.name, fb_cmd_itf, fb_ctrl, following_ctrl_it->info.name); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3346,9 +3853,11 @@ ControllerManager::check_fallback_controllers_state_pre_activation( } else { - message = "Controller with name '" + controller_it->info.name + - "' cannot be activated, as not all of its fallback controller's : '" + - fb_ctrl + "' command interfaces are currently available!"; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as not all of its fallback " + "controller's : '{}' command interfaces are currently available!"), + controller_it->info.name, fb_ctrl); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3380,24 +3889,25 @@ ControllerManager::check_fallback_controllers_state_pre_activation( std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == exported_state_itfs.end()) { - message = "Controller with name '" + controller_it->info.name + - "' cannot be activated, as the state interface : '" + fb_state_itf + - "' required by its fallback controller : '" + fb_ctrl + - "' is not exported by the controller : '" + - following_ctrl_it->info.name + "' in the current fallback list!"; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as the state interface : " + "'{}' required by its fallback controller : '{}' is not exported by the " + "controller : '{}' in the current fallback list!"), + controller_it->info.name, fb_state_itf, fb_ctrl, following_ctrl_it->info.name); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } else { - message = - "Controller with name '" + controller_it->info.name + - "' cannot be activated, as the state interface : '" + fb_state_itf + - "' required by its fallback controller : '" + fb_ctrl + - "' is not available as the controller is not in active state!. May be " - "consider adding this controller to the fallback list of the controller : '" + - following_ctrl_it->info.name + "' or already have it activated."; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as the state interface : '{}' " + "required by its fallback controller : '{}' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '{}' or already have it activated."), + controller_it->info.name, fb_state_itf, fb_ctrl, following_ctrl_it->info.name); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3405,9 +3915,11 @@ ControllerManager::check_fallback_controllers_state_pre_activation( } else { - message = "Controller with name '" + controller_it->info.name + - "' cannot be activated, as not all of its fallback controller's : '" + - fb_ctrl + "' state interfaces are currently available!"; + message = fmt::format( + FMT_COMPILE( + "Controller with name '{}' cannot be activated, as not all of its fallback " + "controller's : '{}' state interfaces are currently available!"), + controller_it->info.name, fb_ctrl); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3452,17 +3964,45 @@ void ControllerManager::publish_activity() controller_interface::return_type ControllerManager::check_for_interfaces_availability_to_activate( const std::vector & controllers, const std::vector activation_list, - std::string & message) + const std::vector deactivation_list, std::string & message) { + std::vector future_unavailable_cmd_interfaces = {}; + std::vector future_available_cmd_interfaces = {}; + for (const auto & controller_name : deactivation_list) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + message = fmt::format( + FMT_COMPILE("Unable to find the deactivation controller : '{}' within the controller list"), + controller_name); + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + const auto controller_cmd_interfaces = + controller_it->c->command_interface_configuration().names; + for (const auto & cmd_itf : controller_cmd_interfaces) + { + future_available_cmd_interfaces.push_back(cmd_itf); + } + } for (const auto & controller_name : activation_list) { + if (ros2_control::has_item(deactivation_list, controller_name)) + { + // skip controllers that are being deactivated and activated in the same request + continue; + } auto controller_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controller_name)); if (controller_it == controllers.end()) { - message = - "Unable to find the controller : '" + controller_name + "' within the controller list"; + message = fmt::format( + FMT_COMPILE("Unable to find the controller : '{}' within the controller list"), + controller_name); RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3476,18 +4016,46 @@ controller_interface::return_type ControllerManager::check_for_interfaces_availa { if (!resource_manager_->command_interface_is_available(cmd_itf)) { - message = "Unable to activate controller '" + controller_it->info.name + - "' since the command interface '" + cmd_itf + "' is not available."; + message = fmt::format( + FMT_COMPILE( + "Unable to activate controller '{}' since the " + "command interface '{}' is not available."), + controller_it->info.name, cmd_itf); + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + if ( + resource_manager_->command_interface_is_claimed(cmd_itf) && + !ros2_control::has_item(future_available_cmd_interfaces, cmd_itf)) + { + message = fmt::format( + FMT_COMPILE( + "Unable to activate controller '{}' since the " + "command interface '{}' is currently claimed by another controller."), + controller_it->info.name, cmd_itf); + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + if (ros2_control::has_item(future_unavailable_cmd_interfaces, cmd_itf)) + { + message = fmt::format( + FMT_COMPILE( + "Unable to activate controller '{}' since the " + "command interface '{}' will be used by another controller that is being activated."), + controller_it->info.name, cmd_itf); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } + future_unavailable_cmd_interfaces.push_back(cmd_itf); } for (const auto & state_itf : controller_state_interfaces) { if (!resource_manager_->state_interface_is_available(state_itf)) { - message = "Unable to activate controller '" + controller_it->info.name + - "' since the state interface '" + state_itf + "' is not available."; + message = fmt::format( + FMT_COMPILE( + "Unable to activate controller '{}' since the state interface '{}' is not available."), + controller_it->info.name, state_itf); RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -3549,8 +4117,8 @@ void ControllerManager::controller_activity_diagnostic_callback( 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(); + const auto periodicity_stats = controllers[i].periodicity_statistics->get_statistics(); + const auto exec_time_stats = controllers[i].execution_time_statistics->get_statistics(); stat.add( controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); const bool publish_periodicity_stats = @@ -3846,7 +4414,7 @@ void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { const std::string periodicity_stat_name = "periodicity"; - const auto cm_stats = periodicity_stats_.GetStatistics(); + const auto cm_stats = periodicity_stats_.get_statistics(); stat.add("update_rate", std::to_string(get_update_rate())); stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); stat.add( @@ -3873,9 +4441,9 @@ void ControllerManager::controller_manager_diagnostic_callback( } 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"; + const std::string diag_summary = fmt::format( + FMT_COMPILE("Controller Manager has bad periodicity : {} Hz. Expected consistent {} Hz"), + cm_stats.average, get_update_rate()); if ( periodicity_error > params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error || @@ -3968,6 +4536,81 @@ void ControllerManager::update_list_with_controller_chain( } } +void ControllerManager::build_controllers_topology_info( + const std::vector & controllers) +{ + std::for_each( + controller_chain_spec_.begin(), controller_chain_spec_.end(), + [](auto & pair) + { + pair.second.following_controllers.clear(); + pair.second.preceding_controllers.clear(); + }); + std::for_each( + controller_chained_reference_interfaces_cache_.begin(), + controller_chained_reference_interfaces_cache_.end(), [](auto & pair) { pair.second.clear(); }); + std::for_each( + controller_chained_state_interfaces_cache_.begin(), + controller_chained_state_interfaces_cache_.end(), [](auto & pair) { pair.second.clear(); }); + for (const auto & controller : controllers) + { + if (is_controller_unconfigured(*controller.c)) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' is unconfigured, skipping chain building.", + controller.info.name.c_str()); + continue; + } + const auto cmd_itfs = controller.c->command_interface_configuration().names; + const auto state_itfs = controller.c->state_interface_configuration().names; + + for (const auto & cmd_itf : cmd_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) + { + ros2_control::add_item( + controller_chain_spec_[controller.info.name].following_controllers, ctrl_it->info.name); + ros2_control::add_item( + controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller.info.name); + ros2_control::add_item( + controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller.info.name); + } + } + // This is needed when we start exporting the state interfaces from the controllers + for (const auto & state_itf : state_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) + { + ros2_control::add_item( + controller_chain_spec_[controller.info.name].preceding_controllers, ctrl_it->info.name); + ros2_control::add_item( + controller_chain_spec_[ctrl_it->info.name].following_controllers, controller.info.name); + ros2_control::add_item( + controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller.info.name); + } + } + } + for (const auto & [controller_name, controller_chain] : controller_chain_spec_) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' has %ld following controllers and %ld preceding controllers.", + controller_name.c_str(), controller_chain.following_controllers.size(), + controller_chain.preceding_controllers.size()); + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain.following_controllers.empty(), "%s", + fmt::format( + "\tFollowing controllers are : {}", fmt::join(controller_chain.following_controllers, ", ")) + .c_str()); + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain.preceding_controllers.empty(), "%s", + fmt::format( + "\tPreceding controllers are : {}", fmt::join(controller_chain.preceding_controllers, ", ")) + .c_str()); + } +} + rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const ControllerSpec & controller) const { @@ -4020,8 +4663,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( } // ensure controller's `use_sim_time` parameter matches controller_manager's - const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); - if (use_sim_time.as_bool()) + if (use_sim_time_) { if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { @@ -4061,7 +4703,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( void ControllerManager::cleanup_controller_exported_interfaces(const ControllerSpec & controller) { - if (is_controller_inactive(controller.c) && controller.c->is_chainable()) + if (!is_controller_active(controller.c) && controller.c->is_chainable()) { RCLCPP_DEBUG( get_logger(), "Removing controller '%s' exported interfaces from resource manager.", diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 4ebe007574..ef9aa1f87d 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -6,6 +6,13 @@ controller_manager: description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." } + enforce_command_limits: { + type: bool, + default_value: true, + read_only: true, + description: "If true, the controller manager will enforce command limits defined in the robot description. If false, no limits will be enforced. If true, when the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits defined in the robot description. If the command is within the limits, the command is passed through without any changes.", + } + hardware_components_initial_state: unconfigured: { type: string_array, @@ -25,6 +32,45 @@ controller_manager: } } + shutdown_on_initial_state_failure: { + type: bool, + default_value: false, + read_only: true, + description: "Specifies whether the controller manager should shut down if setting the desired initial state fails during startup.", + } + + defaults: + switch_controller: + strictness: { + type: string, + default_value: "strict", + description: "The default switch controller strategy. This strategy is used when no strategy is specified in the switch_controller service call.", + validation: { + not_empty<>: null, + one_of<>: [[ + "strict", + "best_effort", + ]], + } + } + + allow_controller_activation_with_inactive_hardware: { + type: bool, + default_value: false, + read_only: true, + description: "If true, controllers are allowed to claim resources from inactive hardware components. If false, controllers can only claim resources from active hardware components. \ + However, it is not recommended to set this parameter to true for the safety reasons with the hardware and unexpected movement, this is purely added for backward compatibility.", + } + + deactivate_controllers_on_hardware_self_deactivate: { + type: bool, + default_value: true, + read_only: true, + description: "If set to true, when a hardware component returns DEACTIVATE on the write cycle, controllers using those interfaces will be deactivated. When set to false, controllers using \ + those interfaces will continue to run. It is not recommended to set this parameter to false for safety reasons with hardware. This will be the default behaviour of the controller \ + manager and this parameter will be removed in future releases. Please use with caution.", + } + diagnostics: threshold: controller_manager: @@ -205,3 +251,8 @@ controller_manager: gt<>: 0.0, } } + overruns: + print_warnings: { + type: bool, + description: "If true, the controller manager will print a warning message to the console if an overrun is detected in its real-time loop (``read``, ``update`` and ``write``). By default, it is set to true, except when used with ``use_sim_time`` parameter set to true.", + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index bba5c1c3b2..ef0f6fb029 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -70,42 +70,42 @@ int main(int argc, char ** argv) } } - rclcpp::Parameter cpu_affinity_param; - if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) - { - std::vector cpus = {}; - if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) - { - cpus = {static_cast(cpu_affinity_param.as_int())}; - } - else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) - { - const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); - std::for_each( - cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), - [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); - } - const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); - if (!affinity_result.first) - { - RCLCPP_WARN( - cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); - } - } - - // wait for the clock to be available - cm->get_clock()->wait_until_started(); - cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); - RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); + const bool manage_overruns = cm->get_parameter_or("overruns.manage", true); + RCLCPP_INFO( + cm->get_logger(), "Overruns handling is : %s", manage_overruns ? "enabled" : "disabled"); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(), thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time]() + [cm, thread_priority, use_sim_time, manage_overruns]() { + rclcpp::Parameter cpu_affinity_param; + if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) + { + std::vector cpus = {}; + if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + cpus = {static_cast(cpu_affinity_param.as_int())}; + } + else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) + { + const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); + std::for_each( + cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), + [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); + } + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); + if (!affinity_result.first) + { + RCLCPP_WARN( + cm->get_logger(), "Unable to set the CPU affinity : '%s'", + affinity_result.second.c_str()); + } + } + if (!realtime_tools::configure_sched_fifo(thread_priority)) { RCLCPP_WARN( @@ -122,6 +122,10 @@ int main(int argc, char ** argv) thread_priority); } + // wait for the clock to be available + cm->get_clock()->wait_until_started(); + cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); + // for calculating sleep time auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); @@ -152,7 +156,7 @@ int main(int argc, char ** argv) { next_iteration_time += period; const auto time_now = std::chrono::steady_clock::now(); - if (next_iteration_time < time_now) + if (manage_overruns && next_iteration_time < time_now) { const double time_diff = static_cast( diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 654240f5f3..5a6d53da1c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -149,6 +149,17 @@ class TestControllerManagerSrvs public: TestControllerManagerSrvs() {} + ~TestControllerManagerSrvs() override + { + RCLCPP_DEBUG(cm_->get_logger(), "Stopping controller manager updater thread"); + stop_runner_ = true; + if (cm_rt_thread_.joinable()) + { + cm_rt_thread_.join(); + } + RCLCPP_DEBUG(cm_->get_logger(), "Controller manager updater thread stopped"); + } + void SetUp() override { ControllerManagerFixture::SetUp(); @@ -157,13 +168,32 @@ class TestControllerManagerSrvs void SetUpSrvsCMExecutor() { - update_timer_ = cm_->create_wall_timer( - std::chrono::milliseconds(10), - [&]() + cm_rt_thread_ = std::thread( + [&] { - cm_->read(time_, PERIOD); - cm_->update(time_, PERIOD); - cm_->write(time_, PERIOD); + // for calculating sleep time + auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_->get_update_rate()); + + // for calculating the measured period of the loop + rclcpp::Time previous_time = cm_->get_clock()->now(); + std::this_thread::sleep_for(period); + std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; + + while (rclcpp::ok() && !stop_runner_) + { + auto const current_time = cm_->get_clock()->now(); + auto const measured_period = current_time - previous_time; + previous_time = current_time; + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + next_iteration_time += period; + if (next_iteration_time < std::chrono::steady_clock::now()) + { + next_iteration_time = std::chrono::steady_clock::now(); + } + std::this_thread::sleep_until(next_iteration_time); + } }); executor_->add_node(cm_); @@ -202,7 +232,8 @@ class TestControllerManagerSrvs } protected: - rclcpp::TimerBase::SharedPtr update_timer_; + std::atomic_bool stop_runner_ = false; + std::thread cm_rt_thread_; std::future executor_spin_future_; }; diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index dd97700897..f377830418 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -129,7 +129,7 @@ controller_interface::return_type TestChainableController::update_and_write_comm state_interfaces_values_[i] = state_interfaces_[i].get_optional().value(); } - return controller_interface::return_type::OK; + return update_return_value; } CallbackReturn TestChainableController::on_init() { return CallbackReturn::SUCCESS; } @@ -172,7 +172,7 @@ CallbackReturn TestChainableController::on_activate( (*msg)->data = reference_interfaces_; } - return CallbackReturn::SUCCESS; + return fail_on_activate ? CallbackReturn::ERROR : CallbackReturn::SUCCESS; } CallbackReturn TestChainableController::on_cleanup( 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 aa6a313acd..02f49dbaeb 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -80,10 +80,11 @@ class TestChainableController : public controller_interface::ChainableController std::vector get_state_interface_data() const; size_t internal_counter; + bool fail_on_activate = false; + controller_interface::return_type update_return_value = controller_interface::return_type::OK; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; - std::vector exported_state_interface_names_; std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index d779354f0a..31a0c8174a 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -67,7 +67,7 @@ controller_interface::return_type TestController::update( } if (is_async()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate()))); + std::this_thread::sleep_for(std::chrono::microseconds(1000000u / (2 * get_update_rate()))); } update_period_ = period; ++internal_counter; @@ -150,6 +150,19 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (activation_processing_time > 0.0) + { + RCLCPP_INFO( + get_node()->get_logger(), "Sleeping for %.3f seconds to simulate activation processing time", + activation_processing_time); + std::this_thread::sleep_for(std::chrono::duration(activation_processing_time)); + } + + return CallbackReturn::SUCCESS; +} + CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) { if (simulate_cleanup_failure) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index a28dfa420e..aa15796323 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -52,6 +52,8 @@ class TestController : public controller_interface::ControllerInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; @@ -67,6 +69,7 @@ class TestController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; + double activation_processing_time = 0.0; bool simulate_cleanup_failure = false; // Variable where we store when shutdown was called, pointer because the controller // is usually destroyed after shutdown diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp new file mode 100644 index 0000000000..c13df1a713 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp @@ -0,0 +1,66 @@ +// Copyright 2021 Department of Engineering Cybernetics, NTNU. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_controller_failed_activate.hpp" + +#include +#include + +#include "lifecycle_msgs/msg/transition.hpp" + +namespace test_controller_failed_activate +{ +TestControllerFailedActivate::TestControllerFailedActivate() +: controller_interface::ControllerInterface() +{ +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_init() +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TestControllerFailedActivate::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_activate(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + // Simply simulate a controller that can not be activated + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace test_controller_failed_activate + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + test_controller_failed_activate::TestControllerFailedActivate, + controller_interface::ControllerInterface) diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp new file mode 100644 index 0000000000..e2f55dcc9f --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 Department of Engineering Cybernetics, NTNU +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ +#define TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ + +#include +#include + +#include "controller_manager/controller_manager.hpp" + +namespace test_controller_failed_activate +{ +// Corresponds to the name listed within the pluginglib xml +constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] = + "controller_manager/test_controller_failed_activate"; +// Corresponds to the command interface to claim +constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity"; +class TestControllerFailedActivate : public controller_interface::ControllerInterface +{ +public: + TestControllerFailedActivate(); + + virtual ~TestControllerFailedActivate() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::INDIVIDUAL, + {TEST_CONTROLLER_COMMAND_INTERFACE}}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; +}; + +} // namespace test_controller_failed_activate + +#endif // TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml new file mode 100644 index 0000000000..80c4e6bef5 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml @@ -0,0 +1,9 @@ + + + + + Controller used for testing + + + + diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 64206303c1..77072b9b1e 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -403,7 +403,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) test_controller->get_lifecycle_state().id()); // configure controller - rclcpp::Parameter update_rate_parameter("update_rate", static_cast(20)); + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(10)); rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true)); test_controller->get_node()->set_parameter(update_rate_parameter); test_controller->get_node()->set_parameter(is_async_parameter); @@ -472,8 +472,11 @@ 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"; + EXPECT_NEAR( + test_controller->update_period_.seconds(), + 1.0 / (static_cast(test_controller->get_update_rate())), 1.e-6) + << "The first trigger cycle should have non-zero period to allow for integration in the " + "controllers"; const double exp_period = (cm_->get_trigger_clock()->now() - time_).seconds(); time_ = cm_->get_trigger_clock()->now(); @@ -495,8 +498,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) } ASSERT_THAT( test_controller->internal_counter, - testing::AllOf( - testing::Ge(last_internal_counter + 18), testing::Le(last_internal_counter + 21))) + testing::AllOf(testing::Ge(last_internal_counter + 9), testing::Le(last_internal_counter + 11))) << "As the sleep is 1 sec and the controller rate is 20Hz, we should have approx. 20 updates"; last_internal_counter = test_controller->internal_counter; @@ -706,12 +708,12 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle_at_cm_rat ASSERT_THAT( test_controller->internal_counter, testing::AllOf( - testing::Ge(last_internal_counter + cm_->get_update_rate() - 3), - testing::Le(last_internal_counter + cm_->get_update_rate() + 1))) + testing::Ge(last_internal_counter + cm_->get_update_rate() - 5), + testing::Le(last_internal_counter + cm_->get_update_rate() + 2))) << "As the sleep is 1 sec and the controller rate is 100Hz, we should have approx. 100 updates"; - // Sleep for 2 cycles to allow for any changes - std::this_thread::sleep_for(std::chrono::milliseconds(20)); + // Sleep for 3 cycles to allow for any changes + std::this_thread::sleep_for(std::chrono::milliseconds(30)); last_internal_counter = test_controller->internal_counter; { ControllerManagerRunner cm_runner(this); @@ -725,7 +727,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle_at_cm_rat ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - std::this_thread::sleep_for(std::chrono::milliseconds(20)); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); EXPECT_EQ(last_internal_counter + 1, test_controller->internal_counter) << "Controller is stopped at the end of update, it should finish it's active cycle"; @@ -910,24 +912,24 @@ 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.8 / cm_update_rate), testing::Lt((1.2 / cm_update_rate)))); + testing::AllOf(testing::Ge(0.65 / cm_update_rate), testing::Lt((1.6 / cm_update_rate)))); ASSERT_EQ( test_controller->internal_counter, - cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + cm_->get_loaded_controllers()[0].execution_time_statistics->get_count()); ASSERT_EQ( test_controller->internal_counter - 1, - cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + cm_->get_loaded_controllers()[0].periodicity_statistics->get_count()) << "The first update is not counted in periodicity statistics"; EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(), testing::AllOf( - testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); + testing::Ge(0.9 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(), testing::AllOf( - testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); + testing::Ge(0.5 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_max(), testing::AllOf( testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((2.0 * cm_->get_update_rate())))); loop_rate.sleep(); @@ -1032,12 +1034,14 @@ TEST_P(TestControllerUpdateRates, check_the_controller_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 double exp_controller_period = + std::round((static_cast(cm_update_rate) / controller_update_rate) - 0.01) / + cm_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); + const auto exp_periodicity = 1.0 / exp_controller_period; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { rclcpp::Time old_time = time; @@ -1054,16 +1058,20 @@ TEST_P(TestControllerUpdateRates, check_the_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()))) + testing::Gt(0.65 * exp_controller_period), + testing::Lt((1.2 * exp_controller_period) + PERIOD.seconds()))) << "update_counter: " << update_counter << " desired controller period: " << controller_period + << " expected controller period: " << exp_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); + EXPECT_NEAR( + test_controller->update_period_.seconds(), + 1.0 / (static_cast(test_controller->get_update_rate())), 1.e-6) + << "The first trigger cycle should have non-zero period to allow for integration in the " + "controllers"; } if (update_counter > 0 && update_counter % cm_update_rate == 0) @@ -1086,22 +1094,22 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) 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()); + cm_->get_loaded_controllers()[0].execution_time_statistics->get_count()); ASSERT_EQ( test_controller->internal_counter - 1, - cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + cm_->get_loaded_controllers()[0].periodicity_statistics->get_count()) << "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)))); + cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(), + testing::AllOf(testing::Ge(0.9 * 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)))); + cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(), + testing::AllOf(testing::Ge(0.5 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_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(), + cm_->get_loaded_controllers()[0].execution_time_statistics->get_average(), 50.0); // 50 microseconds } } @@ -1179,12 +1187,14 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an 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 double exp_controller_period = + std::round((static_cast(cm_update_rate) / controller_update_rate) - 0.01) / + cm_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); + const auto exp_periodicity = 1.0 / exp_controller_period; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { rclcpp::Time old_time = time; @@ -1201,10 +1211,11 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an EXPECT_THAT( test_controller->update_period_.seconds(), testing::AllOf( - testing::Gt(0.99 * controller_period), - testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + testing::Gt(0.65 * exp_controller_period), + testing::Lt((1.05 * exp_controller_period) + PERIOD.seconds()))) << "update_counter: " << update_counter << " desired controller period: " << controller_period + << " expected controller period: " << exp_controller_period << " actual controller period: " << test_controller->update_period_.seconds(); } // else @@ -1232,22 +1243,22 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an 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(), + cm_->get_loaded_controllers()[0].execution_time_statistics->get_count(), testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_count(), testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(), testing::AllOf(testing::Ge(0.9 * exp_periodicity), testing::Lt((1.1 * exp_periodicity)))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(), testing::AllOf(testing::Ge(0.5 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); EXPECT_THAT( - cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + cm_->get_loaded_controllers()[0].periodicity_statistics->get_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(), + cm_->get_loaded_controllers()[0].execution_time_statistics->get_average(), 12000); // more or less 12 milliseconds considering the waittime in the controller } } @@ -2170,3 +2181,859 @@ TEST_F( EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); } } + +class TestControllerManagerControllerChainFailedUpdateCycle +: public ControllerManagerFixture +{ +}; + +TEST_F( + TestControllerManagerControllerChainFailedUpdateCycle, + test_failing_update_cycle_in_a_controller_chain) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration state_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // Controller names + const std::string test_chainable_controller_1_name = "test_chainable_controller_1"; + const std::string test_chainable_controller_2_name = "test_chainable_controller_2"; + const std::string test_chainable_controller_3_name = "test_chainable_controller_3"; + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + // chain controller 1 + auto test_chainable_controller_1 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_1->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_1->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_1->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 2 + auto test_chainable_controller_2 = + std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_1_name + "/modified_joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_2->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 3 + auto test_chainable_controller_3 = + std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_2_name + "/modified_joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_3->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_3->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_3->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // controller 1 + auto test_controller_1 = std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_3_name + "/modified_joint1/position"}; + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_1->set_state_interface_configuration(state_itfs_cfg); + + // controller 2 + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"joint2/velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + + { + cm_->add_controller( + test_chainable_controller_1, test_chainable_controller_1_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_2, test_chainable_controller_2_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_3, test_chainable_controller_3_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, test_controller_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + } + + EXPECT_EQ(5u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller_1.use_count()); + EXPECT_EQ(2, test_chainable_controller_2.use_count()); + EXPECT_EQ(2, test_chainable_controller_3.use_count()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + std::vector start_controllers = { + test_controller_1_name, test_controller_2_name, test_chainable_controller_1_name, + test_chainable_controller_2_name, test_chainable_controller_3_name}; + auto activate_all_controllers = [&]() + { + std::vector stop_controllers = {}; + // Start controller, will take effect at the end of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + }; + activate_all_controllers(); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_3->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + + auto run_10_update_cycles = [&]() + { + // Let's emulate 10 update cycles and it should be working + for (int i = 0; i < 10; ++i) + { + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + } + }; + + auto check_for_inactive_chain = [&]() + { + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_3->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + }; + + start_controllers = { + test_controller_1_name, test_chainable_controller_1_name, test_chainable_controller_2_name, + test_chainable_controller_3_name}; + run_10_update_cycles(); + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + check_for_inactive_chain(); + + test_controller_1->set_external_commands_for_testing({0.0}); + activate_all_controllers(); + run_10_update_cycles(); + test_chainable_controller_1->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain(); + + test_chainable_controller_1->update_return_value = controller_interface::return_type::OK; + activate_all_controllers(); + run_10_update_cycles(); + test_chainable_controller_2->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain(); + + test_chainable_controller_2->update_return_value = controller_interface::return_type::OK; + activate_all_controllers(); + run_10_update_cycles(); + test_chainable_controller_3->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain(); +} + +TEST_F( + TestControllerManagerControllerChainFailedUpdateCycle, + test_failing_update_cycle_in_a_complex_controller_chain) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration state_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // Controller names + const std::string test_chainable_controller_1_name = "test_chainable_controller_1"; + const std::string test_chainable_controller_2_name = "test_chainable_controller_2"; + const std::string test_chainable_controller_3_name = "test_chainable_controller_3"; + const std::string test_chainable_controller_4_name = "test_chainable_controller_4"; + const std::string test_chainable_controller_5_name = "test_chainable_controller_5"; + const std::string test_chainable_controller_6_name = "test_chainable_controller_6"; + const std::string test_chainable_controller_7_name = "test_chainable_controller_7"; + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + // chain controller 1 + auto test_chainable_controller_1 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_1->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_1->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_1->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 2 + auto test_chainable_controller_2 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/max_velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_2->set_reference_interface_names({"modified_joint1/max_velocity"}); + test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 3 + auto test_chainable_controller_3 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint2/velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_3->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_3->set_reference_interface_names({"modified_joint2/velocity"}); + test_chainable_controller_3->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 4 + auto test_chainable_controller_4 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint3/velocity"}; + state_itfs_cfg.names = {"joint3/velocity"}; + test_chainable_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_4->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_4->set_reference_interface_names({"modified_joint3/velocity"}); + test_chainable_controller_4->set_exported_state_interface_names({"modified_joint3/velocity"}); + + // chain controller 5 + auto test_chainable_controller_5 = + std::make_shared(); + cmd_itfs_cfg.names = { + test_chainable_controller_1_name + "/modified_joint1/position", + test_chainable_controller_2_name + "/modified_joint1/max_velocity"}; + state_itfs_cfg.names = {"joint2/velocity", "joint3/velocity"}; + test_chainable_controller_5->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_5->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_5->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller_5->set_exported_state_interface_names({"modified_joint1/velocity"}); + + // chain controller 6 + auto test_chainable_controller_6 = + std::make_shared(); + cmd_itfs_cfg.names = { + test_chainable_controller_3_name + "/modified_joint2/velocity", + test_chainable_controller_4_name + "/modified_joint3/velocity"}; + state_itfs_cfg.names = { + "joint2/velocity", "joint3/velocity", + test_chainable_controller_7_name + "/joint2/max_acceleration"}; + test_chainable_controller_6->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_6->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_6->set_reference_interface_names({"modified_joint2/velocity"}); + test_chainable_controller_6->set_exported_state_interface_names({"modified_joint2/velocity"}); + + // chain controller 7 + auto test_chainable_controller_7 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint2/max_acceleration"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_7->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_7->set_state_interface_configuration(state_itfs_cfg); + test_chainable_controller_7->set_reference_interface_names({"modified_joint2/max_acceleration"}); + test_chainable_controller_7->set_exported_state_interface_names({"joint2/max_acceleration"}); + + // controller 1 + auto test_controller_1 = std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_5_name + "/modified_joint1/position"}; + // this is to create a closed-loop instance in the control design + state_itfs_cfg.names = {test_chainable_controller_1_name + "/modified_joint2/velocity"}; + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_1->set_state_interface_configuration(state_itfs_cfg); + + // controller 2 + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {test_chainable_controller_6_name + "/modified_joint2/velocity"}; + state_itfs_cfg.names = {"joint2/velocity"}; + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + + { + cm_->add_controller( + test_chainable_controller_2, test_chainable_controller_2_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_1, test_chainable_controller_1_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_6, test_chainable_controller_6_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_3, test_chainable_controller_3_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_4, test_chainable_controller_4_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, test_controller_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_5, test_chainable_controller_5_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_7, test_chainable_controller_7_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + + EXPECT_EQ(9u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller_1.use_count()); + EXPECT_EQ(2, test_chainable_controller_2.use_count()); + EXPECT_EQ(2, test_chainable_controller_3.use_count()); + EXPECT_EQ(2, test_chainable_controller_4.use_count()); + EXPECT_EQ(2, test_chainable_controller_5.use_count()); + EXPECT_EQ(2, test_chainable_controller_6.use_count()); + EXPECT_EQ(2, test_chainable_controller_7.use_count()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + auto check_controllers_state = [&](const std::vector state) + { + ASSERT_EQ(state[0], test_chainable_controller_1->get_lifecycle_state().id()); + ASSERT_EQ(state[1], test_chainable_controller_2->get_lifecycle_state().id()); + ASSERT_EQ(state[2], test_chainable_controller_3->get_lifecycle_state().id()); + ASSERT_EQ(state[3], test_chainable_controller_4->get_lifecycle_state().id()); + ASSERT_EQ(state[4], test_chainable_controller_5->get_lifecycle_state().id()); + ASSERT_EQ(state[5], test_chainable_controller_6->get_lifecycle_state().id()); + ASSERT_EQ(state[6], test_chainable_controller_7->get_lifecycle_state().id()); + ASSERT_EQ(state[7], test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ(state[8], test_controller_2->get_lifecycle_state().id()); + }; + check_controllers_state( + std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_6_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_1_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_7_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_2_name)); + ASSERT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_5_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_3_name)); + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_4_name)); + ASSERT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + } + + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)); + + std::vector start_controllers = { + test_controller_1_name, test_controller_2_name, + test_chainable_controller_1_name, test_chainable_controller_2_name, + test_chainable_controller_3_name, test_chainable_controller_4_name, + test_chainable_controller_5_name, test_chainable_controller_6_name, + test_chainable_controller_7_name}; + auto activate_controllers = [&]() + { + std::vector stop_controllers = {}; + // Start controller, will take effect at the end of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + }; + activate_controllers(); + + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + + auto run_5_update_cycles = [&]() + { + // Let's emulate 5 update cycles and it should be working + for (int i = 0; i < 5; ++i) + { + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + } + }; + + auto check_for_inactive_chain_1 = [&]() + { + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + check_controllers_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + }; + + start_controllers = { + test_controller_1_name, test_chainable_controller_1_name, test_chainable_controller_2_name, + test_chainable_controller_5_name}; + run_5_update_cycles(); + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + check_for_inactive_chain_1(); + test_controller_1->set_external_commands_for_testing({0.0}); + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_1->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_1(); + test_chainable_controller_1->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_2->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_1(); + test_chainable_controller_2->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_5->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_1(); + test_chainable_controller_5->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + run_5_update_cycles(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + + // Now let's check the 2nd chain + auto check_for_inactive_chain_2 = [&]() + { + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + check_controllers_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}); + }; + start_controllers = { + test_controller_2_name, test_chainable_controller_3_name, test_chainable_controller_4_name, + test_chainable_controller_6_name, test_chainable_controller_7_name}; + + run_5_update_cycles(); + test_controller_2->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + check_for_inactive_chain_2(); + test_controller_2->set_external_commands_for_testing({0.0}); + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_3->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_2(); + test_chainable_controller_3->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_4->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_2(); + test_chainable_controller_4->update_return_value = controller_interface::return_type::OK; + + activate_controllers(); + check_controllers_state(std::vector(9, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + run_5_update_cycles(); + test_chainable_controller_6->update_return_value = controller_interface::return_type::ERROR; + check_for_inactive_chain_2(); + test_chainable_controller_6->update_return_value = controller_interface::return_type::OK; +} + +class TestControllerManagerChainableControllerFailedActivation +: public ControllerManagerFixture +{ +}; + +TEST_F( + TestControllerManagerChainableControllerFailedActivation, + test_chainable_controllers_failed_activation_and_then_reconfiguring_it) +{ + const std::string test_controller_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // controller 4 + auto test_chainable_controller = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller->set_state_interface_configuration(itfs_cfg); + test_chainable_controller->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller->set_exported_state_interface_names({"modified_joint2/velocity"}); + test_chainable_controller->fail_on_activate = true; + + cm_->add_controller( + test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_NAME, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + + std::vector start_controllers = {test_chainable_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + { + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + // Now, after reconfiguring it, it should work + test_chainable_controller->fail_on_activate = false; + + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + } +} + +TEST_F( + TestControllerManagerChainableControllerFailedActivation, + test_chainable_controllers_failed_activation_stops_all_list) +{ + const std::string test_chainable_controller_2_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + controller_interface::InterfaceConfiguration itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // controller 1 + auto test_chainable_controller = + std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller->set_state_interface_configuration(itfs_cfg); + test_chainable_controller->set_reference_interface_names({"modified_joint1/position"}); + test_chainable_controller->set_exported_state_interface_names({"modified_joint2/velocity"}); + test_chainable_controller->fail_on_activate = false; + + auto test_chainable_controller_2 = + std::make_shared(); + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {"joint2/velocity"}; + test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_chainable_controller_2->set_state_interface_configuration(itfs_cfg); + test_chainable_controller_2->set_reference_interface_names({"modified_joint2/position"}); + test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"}); + test_chainable_controller_2->fail_on_activate = false; + + auto test_controller = std::make_shared(); + cmd_itfs_cfg.names = { + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/modified_joint1/position"}; + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + test_controller->set_state_interface_configuration(itfs_cfg); + + cm_->add_controller( + test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_NAME, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chainable_controller_2, test_chainable_controller_2_name, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_chainable_controller.use_count()); + EXPECT_EQ(2, test_chainable_controller_2.use_count()); + EXPECT_EQ(2, test_controller.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME)); + } + + std::vector start_controllers = { + test_chainable_controller::TEST_CONTROLLER_NAME, test_chainable_controller_2_name, + test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + { + ControllerManagerRunner cm_runner(this); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_state().id()); + + // Now deactivate all the controllers + auto switch_future_2 = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future_2.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::OK, switch_future_2.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + } + + // Now, let's make the first controller fail on activation and see that all the controllers are + // set to inactive in strict mode + test_chainable_controller->fail_on_activate = true; + { + ControllerManagerRunner cm_runner(this); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + } + + // Now, reconfigure and test with BEST_EFFORT strictness and see that the other two controllers + // are active + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + + const auto best_effort_strictness = + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, best_effort_strictness, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chainable_controller_2->get_lifecycle_state().id()); + // test controller needs to be inactive as it depends on the failed controller's interface + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + } +} diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 77e177da27..e1bcc55895 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -47,6 +47,7 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware); FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error); FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_deactivate); FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error); public: @@ -428,6 +429,95 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); } + + // Simulate deactivate in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to READ_DEACTIVATE_VALUE + // (this should be the same as returning ERROR) + test_controller_actuator->set_first_command_interface_value_to = + test_constants::READ_DEACTIVATE_VALUE; + test_controller_system->set_first_command_interface_value_to = + test_constants::READ_DEACTIVATE_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } } TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error) @@ -745,6 +835,209 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e } } +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_deactivate) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate deactivate in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command + // interface to WRITE_DEACTIVATE_VALUE + test_controller_actuator->set_first_command_interface_value_to = + test_constants::WRITE_DEACTIVATE_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens deactivate in hardware and + // "actuator controller" is deactivated but "broadcaster all" should stay active + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers({TEST_CONTROLLER_ACTUATOR_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate deactivate in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to WRITE_DEACTIVATE_VALUE + test_controller_actuator->set_first_command_interface_value_to = + test_constants::WRITE_DEACTIVATE_VALUE; + test_controller_system->set_first_command_interface_value_to = + test_constants::WRITE_DEACTIVATE_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + // here happens deactivate in hardware and + // "actuator controller" is deactivated but "broadcaster's" should stay active + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerManagerWithTestableCM, testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 54c34bd55d..30fb8ec992 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1143,7 +1143,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order { - ControllerManagerRunner cm_runner(this); cm_->add_controller( test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1399,7 +1398,6 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order { - ControllerManagerRunner cm_runner(this); cm_->add_controller( test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1465,7 +1463,6 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; { - ControllerManagerRunner cm_runner(this); for (const auto & controller : ctrls_order) { cm_->configure_controller(controller); @@ -1674,7 +1671,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) std::shuffle(controllers_list.begin(), controllers_list.end(), mers); { - ControllerManagerRunner cm_runner(this); for (auto random_chain_ctrl : random_chainable_controllers_list) { cm_->add_controller( @@ -1697,7 +1693,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) // configure controllers { - ControllerManagerRunner cm_runner(this); for (auto random_ctrl : controllers_list) { cm_->configure_controller(random_ctrl); @@ -2216,3 +2211,470 @@ TEST_F(TestControllerManagerSrvs, switch_controller_failure_behaviour_on_unknown result->controller[0].required_state_interfaces, UnorderedElementsAre("joint1/position", "joint1/velocity", "joint2/position")); } + +TEST_F(TestControllerManagerSrvs, switch_controller_test_activate_asap) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(3u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[1].name, "test_controller_name"); + + // configure controllers + for (const auto & controller : + {TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME, + TEST_CHAINED_CONTROLLER_2}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(3u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, "test_controller_name"); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1); + + // activate controllers all at once + auto res = cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Now test with activate_asap to false + + res = cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, false, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +} + +TEST_F(TestControllerManagerSrvs, switch_controller_controllers_taking_long_time_to_activate) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create non-chained controllers + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + auto test_controller_3 = std::make_shared(); + cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_3->set_command_interface_configuration(cmd_cfg); + test_controller_3->set_state_interface_configuration(state_cfg); + // take 2 seconds to activate + test_controller_3->activation_processing_time = 2.0; + + // add controllers + const std::string test_ctrl_1_name = "test_controller_name_1"; + const std::string test_ctrl_2_name = "test_controller_name_2"; + const std::string test_ctrl_3_name = "test_controller_name_3"; + + cm_->add_controller( + test_controller_1, test_ctrl_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, test_ctrl_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_3, test_ctrl_3_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(3u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, test_ctrl_1_name); + ASSERT_EQ(result->controller[1].name, test_ctrl_2_name); + ASSERT_EQ(result->controller[2].name, test_ctrl_3_name); + + // configure controllers + for (const auto & controller : {test_ctrl_1_name, test_ctrl_2_name, test_ctrl_3_name}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(3u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[2].name, test_ctrl_1_name); + ASSERT_EQ(result->controller[1].name, test_ctrl_2_name); + ASSERT_EQ(result->controller[0].name, test_ctrl_3_name); + + ASSERT_EQ(0u, test_controller_1->internal_counter); + ASSERT_EQ(0u, test_controller_2->internal_counter); + ASSERT_EQ(0u, test_controller_3->internal_counter); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + ASSERT_EQ(0u, test_controller_1->internal_counter); + ASSERT_EQ(0u, test_controller_2->internal_counter); + ASSERT_EQ(0u, test_controller_3->internal_counter); + + // activate non time taking controllers + { + auto res = cm_->switch_controller( + {test_ctrl_1_name, test_ctrl_2_name}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + unsigned int old_counter_1 = test_controller_1->internal_counter; + unsigned int old_counter_2 = test_controller_2->internal_counter; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + ASSERT_GE(test_controller_1->internal_counter, old_counter_1 + 1); + ASSERT_GE(test_controller_2->internal_counter, old_counter_2 + 1); + EXPECT_EQ(old_counter_1 + 1, test_controller_1->internal_counter); + EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + // Now let's activate the time taking controller with activate_asap set to true + old_counter_1 = test_controller_1->internal_counter; + old_counter_2 = test_controller_2->internal_counter; + { + auto res = cm_->switch_controller( + {test_ctrl_3_name}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + // In 2 seconds, the others should do much more cycles but as the switch is happening in the RT + // loop, it blocked the update for the long time. + ASSERT_THAT( + test_controller_1->internal_counter, + testing::AllOf(testing::Gt(old_counter_1), testing::Lt((old_counter_1 + 4)))); + ASSERT_THAT( + test_controller_2->internal_counter, + testing::AllOf(testing::Gt(old_counter_2), testing::Lt((old_counter_2 + 4)))); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Now, let's deactivate and activate again but with activate_asap as false + { + auto res = cm_->switch_controller( + {}, {test_ctrl_3_name}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + old_counter_1 = test_controller_1->internal_counter; + old_counter_2 = test_controller_2->internal_counter; + { + auto res = cm_->switch_controller( + {test_ctrl_3_name}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, + false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + const auto ideal_cycles = + cm_->get_update_rate() * + static_cast(test_controller_3->activation_processing_time); + + ASSERT_GT(test_controller_1->internal_counter, old_counter_1 + ideal_cycles - 20); + ASSERT_GT(test_controller_2->internal_counter, old_counter_2 + ideal_cycles - 20); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + old_counter_1 = test_controller_1->internal_counter; + old_counter_2 = test_controller_2->internal_counter; + const auto old_counter_3 = test_controller_3->internal_counter; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(old_counter_1 + 1, test_controller_1->internal_counter); + EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter); + EXPECT_EQ(old_counter_3 + 1, test_controller_3->internal_counter); +} + +TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_same_interface) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of controllers + static constexpr char TEST_CONTROLLER_1[] = "test_controller_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_2"; + + // create non-chained controller + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/max_velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // add controllers + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(2u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_1); + ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_2); + + // configure controllers + for (const auto & controller : {TEST_CONTROLLER_1, TEST_CONTROLLER_2}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(2u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_2); + ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_1); + + // Check individual activation and they should work fine + auto res = cm_->switch_controller( + {TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, {TEST_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Now test activating controller_2 + res = cm_->switch_controller( + {TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, {TEST_CONTROLLER_2}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Now try activating both the controllers at once, it should fail as they are using same + // interface + res = cm_->switch_controller( + {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::ERROR); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} diff --git a/controller_manager/test/test_controller_manager_with_namespace.cpp b/controller_manager/test/test_controller_manager_with_namespace.cpp index daad24fcfd..0930973c28 100644 --- a/controller_manager/test/test_controller_manager_with_namespace.cpp +++ b/controller_manager/test/test_controller_manager_with_namespace.cpp @@ -34,11 +34,16 @@ class TestControllerManagerWithNamespace void SetUp() { executor_ = std::make_shared(); + hardware_interface::ResourceManagerParams params; + params.update_rate = 100; + params.clock = rm_node_->get_clock(); + params.logger = rm_node_->get_logger(); + params.executor = executor_; + params.robot_description = ros2_control_test_assets::minimal_robot_urdf; + params.node_namespace = TEST_NAMESPACE; cm_ = std::make_shared( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf, rm_node_->get_node_clock_interface(), - rm_node_->get_node_logging_interface(), true), - executor_, TEST_CM_NAME, TEST_NAMESPACE); + std::make_unique(params, true), executor_, TEST_CM_NAME, + TEST_NAMESPACE); run_updater_ = false; } }; @@ -56,6 +61,15 @@ TEST_P(TestControllerManagerWithNamespace, cm_and_controller_in_namespace) EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, test_controller.use_count()); + const auto all_node_names = cm_->get_node_names(); + ASSERT_THAT( + all_node_names, + testing::UnorderedElementsAreArray( + {"/test_namespace/test_controller_manager", "/test_namespace/test_controller_name", + "/test_namespace/test_controller2_name", "/test_namespace/testactuatorhardware", + "/test_namespace/testsensorhardware", "/test_namespace/testsystemhardware", + "/ResourceManager"})); + // setup interface to claim from controllers controller_interface::InterfaceConfiguration cmd_itfs_cfg; cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; diff --git a/controller_manager/test/test_controller_manager_with_resource_manager.cpp b/controller_manager/test/test_controller_manager_with_resource_manager.cpp new file mode 100644 index 0000000000..7310182824 --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_resource_manager.cpp @@ -0,0 +1,122 @@ +#include "test_controller_manager_with_resource_manager.hpp" + +std::shared_ptr ControllerManagerTest::node_ = nullptr; +std::unique_ptr ControllerManagerTest::test_resource_manager_ = nullptr; +std::shared_ptr ControllerManagerTest::executor_ = nullptr; + +using LifecycleCallbackReturn = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +void ControllerManagerTest::SetUp() +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + node_ = std::make_shared("controller_manager_test_node"); + auto clock = node_->get_clock(); + auto logger = node_->get_logger(); + + test_resource_manager_ = std::make_unique(clock, logger); + executor_ = std::make_shared(); +} + +void ControllerManagerTest::TearDown() +{ + node_.reset(); + test_resource_manager_.reset(); + executor_.reset(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_urdf_without_hardware_plugin) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_without_hardware_plugin; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled()); + +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_invalid_urdf) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = R"()"; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled()); + +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_empty_urdf) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ""; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_wrong_plugins) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_with_wrong_plugin; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_no_geometry; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +TEST_F(ControllerManagerTest, init_controller_manager_with_invalid_urdf) +{ + const std::string invalid_urdf = ros2_control_test_assets::invalid_urdf_with_wrong_plugin; + + TestControllerManager cm(executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{}); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + return result; +} \ No newline at end of file diff --git a/controller_manager/test/test_controller_manager_with_resource_manager.hpp b/controller_manager/test/test_controller_manager_with_resource_manager.hpp new file mode 100644 index 0000000000..78fdf7daa3 --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_resource_manager.hpp @@ -0,0 +1,41 @@ +#ifndef CONTROLLER_MANAGER_TEST_HPP_ +#define CONTROLLER_MANAGER_TEST_HPP_ + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/node.hpp" +#include "std_msgs/msg/string.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +class TestControllerManager : public controller_manager::ControllerManager +{ +public: + using ControllerManager::ControllerManager; + + // Expose callbacks + using ControllerManager::robot_description_callback; + + using ControllerManager::is_resource_manager_initialized; + + using ControllerManager::resource_manager_; + + using ControllerManager::get_robot_description_timer; +}; + +class ControllerManagerTest : public ::testing::Test +{ +protected: + static std::shared_ptr node_; + static std::shared_ptr executor_; + static std::unique_ptr test_resource_manager_; + virtual void SetUp(); + virtual void TearDown(); +}; + +#endif \ No newline at end of file diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 67b7cb5b68..122c022813 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -19,6 +19,7 @@ #include "gmock/gmock.h" #include "lifecycle_msgs/msg/state.hpp" #include "test_controller/test_controller.hpp" +#include "test_controller_failed_activate/test_controller_failed_activate.hpp" #include "test_controller_with_interfaces/test_controller_with_interfaces.hpp" using ::testing::_; @@ -80,7 +81,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); - ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + ASSERT_EQ(std::future_status::ready, 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::ERROR, switch_future.get()); @@ -175,7 +176,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) abstract_test_controller2.c->get_lifecycle_state().id()); } - { // Test starting both controllers at the same time + { // Test starting both controllers at the same time, with STRICT both should fail RCLCPP_INFO( cm_->get_logger(), "Starting both controllers at the same time (should notify about resource conflict)"); @@ -184,12 +185,33 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); - ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + ASSERT_EQ(std::future_status::ready, 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::ERROR, switch_future.get()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); + } + + { // Test starting both controllers at the same time, with BEST_EFFORT one should activate + RCLCPP_INFO( + cm_->get_logger(), + "Starting both controllers at the same time (should notify about resource conflict)"); + std::vector start_controllers = {controller_name1, controller_name2}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -330,3 +352,72 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_deactivation_on_ abstract_test_controller2.c->get_lifecycle_state().id()); } } + +TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failure) +{ + std::string controller_type = + test_controller_failed_activate::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME; + + // Load two controllers of different names + std::string controller_name1 = "test_controller1"; + std::string controller_name2 = "test_controller2"; + ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type)); + ASSERT_NO_THROW(cm_->load_controller( + controller_name2, test_controller_with_interfaces::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME)); + ASSERT_EQ(2u, cm_->get_loaded_controllers().size()); + controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers()[0]; + controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers()[1]; + + // Configure controllers + ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name1)); + ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name2)); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); + + { + // Test starting the first controller + // test_controller1 activation always fails + RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); + std::vector start_controllers = {controller_name1}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, STRICT, 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"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); + } + + { + // Test starting the second controller, interfaces should have been released + // test_controller2 always successfully activates + RCLCPP_INFO(cm_->get_logger(), "Starting controller #2"); + std::vector start_controllers = {controller_name2}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, STRICT, 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"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller2.c->get_lifecycle_state().id()); + } +} diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index b382d3b09d..9c9f8892ed 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -37,16 +37,13 @@ from launch_testing.actions import ReadyToTest import launch_testing.markers import launch_ros.actions -from sensor_msgs.msg import JointState import rclpy from controller_manager.test_utils import ( check_controllers_running, - check_if_js_published, check_node_running, ) from controller_manager.launch_utils import generate_controllers_spawner_launch_description -import threading # Executes the given launch file and checks if all nodes can be started @@ -105,17 +102,6 @@ def setUp(self): def tearDown(self): self.node.destroy_node() - def timer_callback(self): - js_msg = JointState() - js_msg.name = ["joint0"] - js_msg.position = [0.0] - self.pub.publish(js_msg) - - def publish_joint_states(self): - self.pub = self.node.create_publisher(JointState, "/joint_states", 10) - self.timer = self.node.create_timer(1.0, self.timer_callback) - rclpy.spin(self.node) - def test_node_start(self): check_node_running(self.node, "controller_manager") @@ -123,17 +109,10 @@ def test_controllers_start(self): cnames = ["ctrl_with_parameters_and_type"] check_controllers_running(self.node, cnames, state="active") - def test_check_if_msgs_published(self): - # we don't have a joint_state_broadcaster in this repo, - # publish joint states manually to test check_if_js_published - thread = threading.Thread(target=self.publish_joint_states) - thread.start() - check_if_js_published("/joint_states", ["joint0"]) - @launch_testing.post_shutdown_test() # These tests are run after the processes in generate_test_description() have shutdown. -class TestDescriptionCraneShutdown(unittest.TestCase): +class TestShutdown(unittest.TestCase): def test_exit_codes(self, proc_info): """Check if the processes exited normally.""" diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 442d389b28..00500e4b41 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -983,6 +983,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) EXPECT_EQ( call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + const auto all_node_names = cm_->get_node_names(); + ASSERT_THAT( + all_node_names, + testing::UnorderedElementsAreArray( + {"/foo_namespace/test_controller_manager", "/foo_namespace/ctrl_1", "/foo_namespace/ctrl_2", + "/ResourceManager", "/foo_namespace/testactuatorhardware", + "/foo_namespace/testsensorhardware", "/foo_namespace/testsystemhardware"})); + auto validate_loaded_controllers = [&]() { auto loaded_controllers = cm_->get_loaded_controllers(); @@ -1144,97 +1152,6 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } -TEST_F( - TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file_deprecated_namespace_arg) -{ - const std::string test_file_path = - std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); - - ControllerManagerRunner cm_runner(this); - // Provide controller type via the parsed file - EXPECT_EQ( - call_spawner( - "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --controller-manager-timeout 1.0 -p " + - test_file_path), - 256) - << "Should fail without the namespacing it"; - EXPECT_EQ( - call_spawner( - "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + - test_file_path + " --ros-args -r __ns:=/random_namespace"), - 256) - << "Should fail when parsed namespace through both way with different namespaces"; - EXPECT_EQ( - call_spawner( - "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + - test_file_path + " --ros-args -r __ns:=/foo_namespace"), - 256) - << "Should fail when parsed namespace through both ways even with same namespacing name"; - EXPECT_EQ( - call_spawner( - "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + - test_file_path), - 0) - << "Should work when parsed through the deprecated arg"; - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); - - auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); - ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ( - 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_array()[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, "ns_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); - ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") - .as_string_array()[0], - test_file_path); - - EXPECT_EQ( - call_spawner( - "ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace " - "-p " + - test_file_path), - 256) - << "Should fail as no type is defined!"; - // Will still be same as the current call will fail - ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); - - auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); - ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - 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_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_array()[0], test_file_path); -} - TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) { const std::string test_file_path = @@ -1318,16 +1235,14 @@ TEST_F( << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + - test_file_path), + "ctrl_with_parameters_and_type --load-only -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry"; EXPECT_EQ( call_spawner( - "chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + - test_file_path), + "chainable_ctrl_with_parameters_and_type --load-only -c test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 0) << "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry"; diff --git a/controller_manager/test/test_test_utils.py b/controller_manager/test/test_test_utils.py new file mode 100644 index 0000000000..c1e3623a1e --- /dev/null +++ b/controller_manager/test/test_test_utils.py @@ -0,0 +1,76 @@ +# Copyright (c) 2025 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import unittest +from sensor_msgs.msg import JointState + +import rclpy +from controller_manager.test_utils import ( + check_if_js_published, +) +import threading + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + def tearDown(self): + self.node.destroy_node() + + def timer_callback(self): + js_msg = JointState() + js_msg.name = ["joint0"] + js_msg.position = [0.0] + self.pub.publish(js_msg) + + def publish_joint_states(self): + self.pub = self.node.create_publisher(JointState, "/joint_states", 10) + self.timer = self.node.create_timer(1.0, self.timer_callback) + rclpy.spin(self.node) + + def test_check_if_msgs_published(self): + # we don't have a joint_state_broadcaster in this repo, + # publish joint states manually to test check_if_js_published + thread = threading.Thread(target=self.publish_joint_states) + thread.start() + check_if_js_published("/joint_states", ["joint0"]) + + # Note: Other test cases are in test_ros2_control_node_launch.py diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index de70b3399b..544d43f971 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ + +5.7.0 (2025-10-03) +------------------ + +5.6.0 (2025-08-26) +------------------ +* Update doc to clarify BEST_EFFORT behavior when switching controllers (`#2448 `_) +* Fix typos in the documentation of SwitchController strictness (`#2445 `_) +* Contributors: Johannes Huemer, Peter Mitrano (AR) + +5.5.0 (2025-07-31) +------------------ + +5.4.0 (2025-07-21) +------------------ + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ +* Cleanup deprecations in `ros_control` (`#2258 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* Add `data_type` field to the HardwareInterfaces message (`#2204 `_) +* Add new strictness modes to SwitchController service (`#2224 `_) +* Contributors: Sai Kishor Kothakota + 4.29.0 (2025-05-04) ------------------- diff --git a/controller_manager_msgs/msg/HardwareComponentState.msg b/controller_manager_msgs/msg/HardwareComponentState.msg index 80b0981894..7b8e66fb94 100644 --- a/controller_manager_msgs/msg/HardwareComponentState.msg +++ b/controller_manager_msgs/msg/HardwareComponentState.msg @@ -2,7 +2,6 @@ string name # Name of the hardware component string type # Type of the hardware component bool is_async # If the hardware component is running asynchronously uint16 rw_rate # read/write rate of the hardware component in Hz -string class_type # DEPRECATED string plugin_name # The name of the plugin that is used to load the hardware component lifecycle_msgs/State state # State of the hardware component HardwareInterface[] command_interfaces # Command interfaces of the hardware component diff --git a/controller_manager_msgs/msg/HardwareInterface.msg b/controller_manager_msgs/msg/HardwareInterface.msg index 80807ef859..219c097e99 100644 --- a/controller_manager_msgs/msg/HardwareInterface.msg +++ b/controller_manager_msgs/msg/HardwareInterface.msg @@ -1,3 +1,4 @@ string name +string data_type bool is_available bool is_claimed diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index a451a0a77a..78cc58abd9 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.29.0 + 6.0.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv index f000aaf61a..a9c8a28d06 100644 --- a/controller_manager_msgs/srv/SwitchController.srv +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -1,23 +1,41 @@ -# The SwitchController service allows you deactivate a number of controllers +# The SwitchController service allows you to deactivate a number of controllers # and activate a number of controllers, all in one single timestep of the # controller manager's control loop. # To switch controllers, specify # * the list of controller names to activate, # * the list of controller names to deactivate, and -# * the strictness (BEST_EFFORT or STRICT) +# * the strictness (STRICT, BEST_EFFORT, AUTO, or FORCE_AUTO) # * STRICT means that switching will fail if anything goes wrong (an invalid -# controller name, a controller that failed to activate, etc. ) -# * BEST_EFFORT means that even when something goes wrong with on controller, -# the service will still try to activate/stop the remaining controllers +# controller name, a controller that failed to activate, etc.). +# * BEST_EFFORT: +# Transitions are only triggered if the controller is not yet in the target state. +# If it is already in the target state, no error occurs. +# Returns 'false' if all controllers of 'activate_controllers' or 'deactivate_controllers' are not yet loaded or configured, +# or the pending transitions of all existing controllers of 'activate_controllers' +# or 'deactivate_controllers' fail. +# Returns 'true' otherwise. +# * AUTO means that the controller manager will automatically resolve the controller +# chain in order to activate and/or deactivate the specified controllers. +# This is useful in complex systems when you want all dependent controllers to start +# within the same update iteration. +# * FORCE_AUTO means that the controller manager will take all necessary steps to activate +# the specified controllers without requiring you to explicitly list the controllers +# to be deactivated. This is useful when the controller being activated depends on +# another unknown controller that is currently running. The controller manager will +# deactivate any controllers that block the activation of the requested controller, +# following the mutually exclusive joint interface switching principle. For example, +# to activate a controller that uses a joint's position interface, the controller manager +# will automatically deactivate any controllers that use conflicting interfaces for +# the same joint. # * activate the controllers as soon as their hardware dependencies are ready, will # wait for all interfaces to be ready otherwise # * the timeout before aborting pending controllers. Zero for infinite # The return value "ok" indicates if the controllers were switched -# successfully or not. The meaning of success depends on the +# successfully or not. The meaning of success depends on the # specified strictness. -# The return value "message" provides some human-readable information +# The return value "message" provides some human-readable information. string[] activate_controllers @@ -25,6 +43,8 @@ string[] deactivate_controllers int32 strictness int32 BEST_EFFORT=1 int32 STRICT=2 +int32 AUTO=3 +int32 FORCE_AUTO=4 bool activate_asap builtin_interfaces/Duration timeout --- diff --git a/doc/debugging.rst b/doc/debugging.rst index 0fef43d11d..acf6c646ff 100644 --- a/doc/debugging.rst +++ b/doc/debugging.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/debugging.rst + Debugging ^^^^^^^^^ diff --git a/doc/introspection.rst b/doc/introspection.rst index 0950cef13a..35fe77ee89 100644 --- a/doc/introspection.rst +++ b/doc/introspection.rst @@ -1,3 +1,4 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/introspection.rst Introspection of the ros2_control setup *************************************** @@ -18,6 +19,15 @@ The topic ``~/introspection_data/full`` can be used to integrate with your custo .. note:: If you have a high frequency of data, it is recommended to use the ``~/introspection_data/names`` and ``~/introspection_data/values`` topic. So, that the data transferred and stored is minimized. +Along with the above introspection data, the ``controller_manager`` also publishes the statistics of the execution time and periodicity of the read and write cycles of the hardware components and the update cycle of the controllers. This is done by registering the statistics of these variables and publishing them on the ``~/statistics`` topic. + +All the registered variables are published over 3 topics: ``~/statistics/full``, ``~/statistics/names``, and ``~/statistics/values``. +- The ``~/statistics/full`` topic publishes the full introspection data along with names and values in a single message. This can be useful to track or view variables and information from command line. +- The ``~/statistics/names`` topic publishes the names of the registered variables. This topic contains the names of the variables registered. This is only published every time a a variables is registered and unregistered. +- The ``~/statistics/values`` topic publishes the values of the registered variables. This topic contains the values of the variables registered. + +This topic is mainly used to introspect the behaviour of the realtime loops, this is very crucial for hardware that need to meet strict deadlines and also to understand the which component of the ecosystem is consuming more time in the realtime loop. + How to introspect internal variables of controllers and hardware components ============================================================================ diff --git a/doc/migration.rst b/doc/migration.rst index 614dbc8a97..3bb3483101 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -1,190 +1,16 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/migration.rst + +Migration Guides: Kilted Kaiju to Lyrical Luth +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases, where changes to user code might be necessary. + -Iron to Jazzy -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ controller_interface ******************** -* ``update_reference_from_subscribers()`` method got time and period parameters `(PR #846) `__. -* The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. - In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments. -* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. -* The ``exported_state_interfaces_`` and ``exported_reference_interfaces_`` now uses the the full interface name as the key to the unordered_map, instead of the interface type. This is related to (`#2038 `_). For instance, the key would be ``joint_1/position`` instead of ``position``. controller_manager ****************** -* Rename ``class_type`` to ``plugin_name`` (`#780 `_) -* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). As a consequence, when using multiple controller managers, you have to remap the topic within the launch file, an example for a python launch file: - - .. code-block:: python - - remappings=[ - ('/robot_description', '/custom_1/robot_description'), - ] - -* Changes from `(PR #1256) `__ - - * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: - - The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. - - This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. - - * The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of - - .. code-block:: xml - - - - - - 0.15 - - - - - - right_finger_joint - 1 - - - - - - - - define your mimic joints directly in the joint definitions: - - .. code-block:: xml - - - - - - - - - - - - - - - - -* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). Use ``robot_description`` topic instead, e.g., you can use the `robot_state_publisher `_. For an example, see `this PR `_ where the change was applied to the demo repository. hardware_interface ****************** -* ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_) -* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. -* A new ``get_optional`` that returns a ``std::optional`` was added to the ``CommandInterface`` and ``StateInterface``. This can be used to check if the value is available or not. (`#1976 `_ and `#2061 `_) - -Adaption of Command-/StateInterfaces -*************************************** - -* The handles for ``Command-/StateInterfaces`` have new set/get methods to access the values. -* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which get parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). The memory is now allocated in the handle itself. - -Access to Command-/StateInterfaces ----------------------------------- - -Earlier code will issue compile-time warnings like: - -.. code:: - - warning: ‘double hardware_interface::Handle::get_value() const’ is deprecated: Use std::optional get_optional() or bool get_value(double & value) instead to retrieve the value. [-Wdeprecated-declarations] - warning: ignoring return value of ‘bool hardware_interface::Handle::set_value(const T&) [with T = double]’ [-Wunused-result] - -The old methods are deprecated and will be removed in the future. The new methods are: - - * ``std::optional get_optional()`` for getting the value. - * ``bool set_value(const T & value)`` for setting the value. - -The return value ``bool`` or ``std::optional`` with ``get_value`` can be used to check if the value is available or not. Similarly, the ``set_value`` method returns a ``bool`` to check if the value was set or not. -The ``get_value`` method will return an empty ``std::nullopt`` or ``false`` if the value is not available. The ``set_value`` method will return ``false`` if the value was not set. - -.. note:: - Checking the result of these operations is important as the value might not be available or the value might not be set. - This is usually the case when the ros2_control framework has some asynchronous operations due to asynchronous controllers or asynchronous hardware components where different threads are involved to access the same data. - -Migration of Command-/StateInterfaces -------------------------------------- -To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: - -1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. -2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.: - - * If you have a ``std::vector hw_commands_;`` for joints' ``CommandInterfaces`` delete the definition and any usage/appearance. - * Wherever you iterated over a state/command or accessed commands like this: - -.. code-block:: c++ - - // states - for (uint i = 0; i < hw_states_.size(); i++) - { - hw_states_[i] = 0; - auto some_state = hw_states_[i]; - } - - // commands - for (uint i = 0; i < hw_commands_.size(); i++) - { - hw_commands_[i] = 0; - auto some_command = hw_commands_[i]; - } - - // specific state/command - hw_commands_[x] = hw_states_[y]; - -replace it with - -.. code-block:: c++ - - // states replace with this - for (const auto & [name, descr] : joint_state_interfaces_) - { - set_state(name, 0.0); - auto some_state = get_state(name); - } - - //commands replace with this - for (const auto & [name, descr] : joint_commands_interfaces_) - { - set_command(name, 0.0); - auto some_command = get_command(name); - } - - // replace specific state/command, for this you need to store the names which are strings - // somewhere or know them. However be careful since the names are fully qualified names which - // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity - set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); - -Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` XML-tag --------------------------------------------------------------------------------------- -If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: - -1. Override the ``virtual std::vector export_unlisted_command_interfaces()`` or ``virtual std::vector export_unlisted_state_interfaces()`` -2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function, add it to a vector and return the vector: - - .. code-block:: c++ - - std::vector my_unlisted_interfaces; - - InterfaceInfo unlisted_interface; - unlisted_interface.name = "some_unlisted_interface"; - unlisted_interface.min = "-5.0"; - unlisted_interface.data_type = "double"; - my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); - - return my_unlisted_interfaces; - -3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. -4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. - -Custom export of Command-/StateInterfaces ----------------------------------------------- -In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - -* If you want to have unlisted interfaces available you need to call the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. -* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``std::shared_ptr`` and the resource manager will not provide access to the created ``Command-/StateInterface`` for the hardware. So you must take care of storing them yourself. -* Names must be unique! diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4322b22655..5f031f15ba 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -1,205 +1,25 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst -Iron to Jazzy -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Release Notes: Kilted Kaiju to Lyrical Luth +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -General -******* -* A ``version.h`` file will be generated per package using the ament_generate_version_header (`#1449 `_). For example, you can include the version of the package in the logs. - - .. code-block:: cpp - - #include - ... - RCLCPP_INFO(get_logger(), "controller_manager version: %s", CONTROLLER_MANAGER_VERSION_STR); +This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases. controller_interface ******************** -For details see the controller_manager section. - -* Pass URDF to controllers on init (`#1088 `_). -* Pass controller manager update rate on the init of the controller interface (`#1141 `_) -* A method to get node options to setup the controller node #api-breaking (`#1169 `_) -* Export state interfaces from the chainable controller #api-breaking (`#1021 `_) -* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. -* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. -* The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) -* The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) -* The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) -* The controllers can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) -* A new ``SemanticComponentCommandInterface`` semantic component provides capabilities analogous to the ``SemanticComponentInterface``, but for write-only devices (`#1945 `_) -* The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 `_) +* The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. (`#2627 `__) +* The controller_manager will now deactivate the entire controller chain if a controller in the chain fails during the update cycle. `(#2681 `__) controller_manager ****************** -* URDF is now passed to controllers on init (`#1088 `_) - This should help avoiding extra legwork in controllers to get access to the ``/robot_description``. -* Pass controller manager update rate on the init of the controller interface (`#1141 `_) -* Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_) -* Set chained controller interfaces 'available' for activated controllers (`#1098 `_) - - * Configured chainable controller: Listed exported interfaces are unavailable and unclaimed - * Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed - * Active chainable controller (in chained mode): Listed exported interfaces are available and claimed -* Try using SCHED_FIFO on any kernel (`#1142 `_) -* A method to get node options to setup the controller node was added (`#1169 `_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options -* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). -* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_) -* Changes from `(PR #1256) `__ - - * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: - - The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. - - This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. +* The default strictness for ``switch_controller`` is changed to ``strict``. (`#2742 `__) - * The syntax for mimic joints is changed to the `official URDF specification `__. - - .. code-block:: xml - - - - - - - - - - - - - - - - - - The parameters within the ``ros2_control`` tag are not supported any more. -* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). -* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). -* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). -* Added support for the wildcard entries for the controller configuration files (`#1724 `_). -* The spawner now supports 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 `_). -* The ``pal_statistics`` is now integrated into the controller_manager, so that the controllers, hardware components and the controller_manager can be easily introspected and monitored using the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` (`#1918 `_). -* A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). -* A new parameter ``enforce_command_limits`` is introduced to be able to enable and disable the enforcement of the command limits (`#1989 `_). -* A latched topic ``~/activity`` has been added to the controller_manager to publish the activity of the controller_manager, where the change in states of the controllers and the hardware components are published. (`#2006 `_). -* The controller manager will use a monotonic clock for triggering read-update-write cycles, but when the ``use_sim_time`` parameter is set to true, it will use the ROS Clock for triggering. When monotonic clock is being used, all the hardware components will receive the monotonic time in their read and write method, instead the controllers will always receive the ROS time in their update method irrespective of the clock being used. (`#2046 `_). hardware_interface ****************** -* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) -* ``test_components`` was moved to its own package (`#1325 `_) -* The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 `_) - - .. code:: xml - - - - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware - 2.0 - 3.0 - 2.0 - - - - -1 - 1 - - - - - - - - -* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) -* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) -* The ``ros2_control`` tag now supports parsing read/write rate ``rw_rate`` for the each hardware component parsed through the URDF (`#1570 `_) - - .. code:: xml - - - - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware - 2.0 - 3.0 - 2.0 - - - - - - - - - - - - - - ros2_control_demo_hardware/MultimodalGripper - - - - 0 - 100 - - - - - - - - - -* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) -* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. -* With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. -* With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. -* 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. -* The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) -* Added new ``get_optional`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 `_ and `#2061 `_) -* Added hardware components execution time and periodicity statistics diagnostics (`#2086 `_) - -joint_limits -************ -* Add header to import limits from standard URDF definition (`#1298 `_) - -Adaption of Command-/StateInterfaces -*************************************** -Changes from `(PR #1688) `_ for an overview of related changes and discussion refer to `(PR #1240) `_. - -* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). -* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. -* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``. - ros2controlcli ************** -* Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 `_) -* The ``set_hardware_component_state`` verb was added (`#1248 `_). Use the following command to set the state of a hardware component - - .. code-block:: bash - - ros2 control set_hardware_component_state - -* The ``load_controller`` now supports parsing of the params file (`#1703 `_). - - .. code-block:: bash - - ros2 control load_controller - -* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 `_). - - .. code-block:: bash - - ros2 control --ros-args -r __ns:= +transmission_interface +********************** +* The ``simple_transmission`` and ``differential_transmission`` now also support the ``force`` interface (`#2588 `_). diff --git a/downstream.humble.repos b/downstream.humble.repos deleted file mode 100644 index d4ee6ec81a..0000000000 --- a/downstream.humble.repos +++ /dev/null @@ -1,13 +0,0 @@ -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 deleted file mode 100644 index 13f06e7b44..0000000000 --- a/downstream.jazzy.repos +++ /dev/null @@ -1,13 +0,0 @@ -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 deleted file mode 100644 index 13f06e7b44..0000000000 --- a/downstream.rolling.repos +++ /dev/null @@ -1,13 +0,0 @@ -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 c60c9a7669..29831098b8 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,105 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ +* Deactivate the controller chain upon failed group activation (`#2669 `_) +* Cleanup GenericSystem component code :broom: (`#2706 `_) +* Fix dynamics calculation of GenericSystem component (`#2705 `_) +* Add `has_state` and `has_command` methods to hardware_component_interface (`#2701 `_) +* [GenericSystem] Initialize joint_control_mode\_ in on_configure (`#2693 `_) +* Cleanup deprecations (`#2589 `_) +* Prepare GenericSystem for other interface data types (`#2571 `_) +* Fix the same hardware component node naming issue with multiple controller managers setup (`#2657 `_) +* Deprecate thread_priority again (`#2644 `_) +* Add magnetic_field_sensor semantic component (`#2627 `_) +* Fix `RCLCPP_WARN_ONCE` within handle (`#2630 `_) +* Fix warnings of uninitialized registry in GenericSystem tests (`#2635 `_) +* Contributors: Aarav Gupta, Christoph Fröhlich, Felix Exner, Sai Kishor Kothakota + +5.7.0 (2025-10-03) +------------------ +* Cleanup deprecations for kilted release (`#2605 `_) +* [CM] Ability to switch controllers in non-realtime loop (`#2452 `_) +* Addition of Default Publisher for `HardwareStatus` Messages (`#2476 `_) +* Add `ControllerInterfaceParams` to initialize the Controllers (`#2390 `_) +* Add detach async policy for rate critical frameworks (`#2477 `_) +* Add parameter to allow controllers with inactive hardware components (`#2501 `_) +* Fix shadowed class member in GenericSystem (`#2561 `_) +* remove virtual from the add_measurement method (`#2558 `_) +* Fix the reloading controller with failed activation (`#2544 `_) +* Fix percentage calculation of Loaned*Interface warnings (`#2542 `_) +* Fix interface configuration docs (`#2537 `_) +* Publish controller manager statistics to better introspect the timings (`#2449 `_) +* Enable logger service for hardware component node (`#2503 `_) +* Fix runtime variant access bug in `HardwareComponentInterface::get_command` helper method (`#2491 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Soham Patil, bijoua29, vvel-tracpilot + +5.6.0 (2025-08-26) +------------------ +* Remove extra semicolons (`#2478 `_) +* Docs: clarify getter comments to reference HardwareComponentInterface (`#2471 `_) +* Start of Unification for `Sensor`, `Actuator`, and `System` into a Single Class (`#2451 `_) (`#2459 `_) +* Unify `write` behavior between Actuator and System hardware interfaces (`#2453 `_) +* Revert "Start of Unification for `Sensor`, `Actuator`, and `System` into a Si…" (`#2456 `_) +* Start of Unification for `Sensor`, `Actuator`, and `System` into a Single Class (`#2451 `_) +* Fix docstring for hardware lifecycle (`#2429 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Soham Patil, Tapia Danish, rishitej04 + +5.5.0 (2025-07-31) +------------------ +* Fix the prepare_command_mode_switch behaviour when HW is INACTIVE (`#2347 `_) +* Fix the joint limiter exception while configuring component (`#2416 `_) +* Migration notes on init (`#2412 `_) +* Contributors: Felix Exner (fexner), Sai Kishor Kothakota + +5.4.0 (2025-07-21) +------------------ +* Delete copy constructor and copy and move operators (`#2378 `_) +* Fix the crashing joint limiters when used with multiple interfaces (`#2371 `_) +* add changes to cast the other data types to double (`#2360 `_) +* Addition of a Default Node for Hardware Component (`#2348 `_) +* Add pixi workflow and dependency file (`#2338 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Soham Patil + +5.3.0 (2025-07-02) +------------------ +* Add deprecations to old methods not using param structs (`#2344 `_) +* expose get_data_type method in loaned interfaces (`#2351 `_) +* Cleanup old internal API (`#2346 `_) +* Improve lexical casts methods (`#2343 `_) +* added params approach to allow propagation in gz_ros2_control (`#2340 `_) +* Deactivate controllers with command interfaces to hardware on DEACTIVATE (`#2334 `_) +* Shift to Struct based Method and Constructors, with Executor passed from CM to `on_init()` (`#2323 `_) +* Add string array to lexical casts (`#2333 `_) +* Contributors: Jordan Palacios, Marq Rasmussen, Sai Kishor Kothakota, Soham Patil + +5.2.0 (2025-06-07) +------------------ +* hardware_interface: optimise & rename find_common_hardware_interfaces (`#2294 `_) +* also use std::mutex on macOS (`#2313 `_) +* Use std::mutex on windows (`#2311 `_) +* Contributors: Christoph Fröhlich, Daisuke Nishimatsu, Eldgar + +5.1.0 (2025-05-24) +------------------ +* [RM] Isolate start and stop interfaces for each Hardware Component (`#2120 `_) +* Use target_link_libraries instead of ament_target_dependencies (`#2266 `_) +* Add new `Handle` constructor for easier initialization (`#2253 `_) +* Cleanup deprecations in `ros_control` (`#2258 `_) +* Read `data_type` for all types of interfaces (`#2235 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* Statically allocate string concatenations using FMT formatting (`#2205 `_) +* Suppress the deprecation warnings of the hardware_interface API (`#2223 `_) +* Add `data_type` field to the HardwareInterfaces message (`#2204 `_) +* Contributors: Sai Kishor Kothakota, mini-1235 + 4.29.0 (2025-05-04) ------------------- * [Diagnostics] Add diagnostics of execution time and periodicity of the hardware components (`#2086 `_) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f43aa10702..e10f7ec16f 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS joint_limits urdf pal_statistics + fmt ) find_package(ament_cmake REQUIRED) @@ -28,19 +29,32 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(hardware_interface SHARED - src/actuator.cpp src/component_parser.cpp src/resource_manager.cpp - src/sensor.cpp - src/system.cpp + src/hardware_component.cpp src/lexical_casts.cpp ) target_compile_features(hardware_interface PUBLIC cxx_std_17) target_include_directories(hardware_interface PUBLIC $ $ + ${pal_statistics_INCLUDE_DIRS} ) -ament_target_dependencies(hardware_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(hardware_interface PUBLIC + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + pluginlib::pluginlib + urdf::urdf + realtime_tools::realtime_tools + rcutils::rcutils + rcpputils::rcpputils + ${joint_limits_TARGETS} + ${TinyXML2_LIBRARIES} + ${tinyxml2_vendor_LIBRARIES} + ${pal_statistics_LIBRARIES} + ${control_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} + fmt::fmt) add_library(mock_components SHARED src/mock_components/generic_system.cpp @@ -51,7 +65,6 @@ target_include_directories(mock_components PUBLIC $ ) target_link_libraries(mock_components PUBLIC hardware_interface) -ament_target_dependencies(mock_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file( hardware_interface mock_components_plugin_description.xml) @@ -63,31 +76,30 @@ if(BUILD_TESTING) ament_add_gmock(test_macros test/test_macros.cpp) target_include_directories(test_macros PRIVATE include) - ament_target_dependencies(test_macros rcpputils) + target_link_libraries(test_macros rcpputils::rcpputils) ament_add_gmock(test_inst_hardwares test/test_inst_hardwares.cpp) - target_link_libraries(test_inst_hardwares hardware_interface) - ament_target_dependencies(test_inst_hardwares rcpputils) + target_link_libraries(test_inst_hardwares hardware_interface rcpputils::rcpputils) ament_add_gmock(test_joint_handle test/test_handle.cpp) - target_link_libraries(test_joint_handle hardware_interface) - ament_target_dependencies(test_joint_handle rcpputils) + target_link_libraries(test_joint_handle hardware_interface rcpputils::rcpputils) # Test helper methods ament_add_gmock(test_helpers test/test_helpers.cpp) target_link_libraries(test_helpers hardware_interface) + # Test lexical casts methods + ament_add_gtest(test_lexical_casts test/test_lexical_casts.cpp) + target_link_libraries(test_lexical_casts hardware_interface) + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) - target_link_libraries(test_component_interfaces hardware_interface) - ament_target_dependencies(test_component_interfaces ros2_control_test_assets) + target_link_libraries(test_component_interfaces hardware_interface ros2_control_test_assets::ros2_control_test_assets) ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) - target_link_libraries(test_component_interfaces_custom_export hardware_interface) - ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + target_link_libraries(test_component_interfaces_custom_export hardware_interface ros2_control_test_assets::ros2_control_test_assets) ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser hardware_interface) - ament_target_dependencies(test_component_parser ros2_control_test_assets) + target_link_libraries(test_component_parser hardware_interface ros2_control_test_assets::ros2_control_test_assets) add_library(test_hardware_components SHARED test/test_hardware_components/test_single_joint_actuator.cpp @@ -97,8 +109,6 @@ if(BUILD_TESTING) test/test_hardware_components/test_system_with_command_modes.cpp ) target_link_libraries(test_hardware_components hardware_interface) - ament_target_dependencies(test_hardware_components - pluginlib) install(TARGETS test_hardware_components DESTINATION lib ) @@ -108,11 +118,7 @@ if(BUILD_TESTING) ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) target_include_directories(test_generic_system PRIVATE include) - target_link_libraries(test_generic_system hardware_interface) - ament_target_dependencies(test_generic_system - pluginlib - ros2_control_test_assets - ) + target_link_libraries(test_generic_system hardware_interface ros2_control_test_assets::ros2_control_test_assets) endif() install( diff --git a/hardware_interface/doc/asynchronous_components.rst b/hardware_interface/doc/asynchronous_components.rst index d0a8f1ed2c..863f2d4081 100644 --- a/hardware_interface/doc/asynchronous_components.rst +++ b/hardware_interface/doc/asynchronous_components.rst @@ -13,7 +13,15 @@ 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``. + +Under the ``ros2_control`` tag, a ``properties`` tag can be added to specify the following parameters of the asynchronous hardware component: + * ``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. +* ``affinity``: (optional) The CPU affinity of the thread that runs the hardware component. The affinity is a list of CPU core IDs. The default value is an empty list, which means that the thread can run on any CPU core. +* ``scheduling_policy``: (optional) The scheduling policy of the thread that runs the hardware component. The scheduling policy can be one of the following values: + * ``synchronized`` (default): The thread will run with the synchronized with the main controller_manager thread. The controller_manager is responsible for triggering the read and write calls of the hardware component. + * ``detached``: The thread will run independently of the main controller_manager thread. The hardware component will manage its own timing for triggering the read and write calls. +* ``print_warnings``: (optional) If set to ``true``, a warning will be printed if the thread is not able to meet its timing requirements. Default is ``true``. .. note:: The thread priority is only used when the hardware component is run asynchronously. @@ -59,7 +67,10 @@ For a RRBot with multimodal gripper and external sensor: - + + + + ros2_control_demo_hardware/MultimodalGripper diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 05e99e2197..065752cb77 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -41,7 +41,7 @@ The hardware transitions to the following state after each method: * **INACTIVE** (``on_configure``, ``on_deactivate``): Communication with the hardware is established and hardware component is configured. - States can be read and command interfaces (System and Actuator only) are available. + States can be read, but command interfaces (System and Actuator only) are not available. As of now, it is left to the hardware component implementation to continue using the command received from the ``CommandInterfaces`` or to skip them completely. diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index f0b7ce36ae..d9662581c4 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -25,13 +25,12 @@ A generic example which shows the structure is provided below. More specific exa value - - + + + -1 1 0.0 - - 5 some_value other_value diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 8eca65c52b..f5cd6b1196 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -76,7 +76,7 @@ calculate_dynamics (optional; boolean; default: false) Calculation of states from commands by using Euler-forward integration or finite differences. custom_interface_with_following_offset (optional; string; default: "") - Mapping of offsetted commands to a custom interface. + Mapping of offsetted commands to a custom interface (see ``position_state_following_offset``). disable_commands (optional; boolean; default: false) Disables mirroring commands to states. @@ -92,7 +92,9 @@ mock_sensor_commands (optional; boolean; default: false) Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. position_state_following_offset (optional; double; default: 0.0) - Following offset added to the commanded values when mirrored to states. Only applied, if ``custom_interface_with_following_offset`` is false. + Following offset added to the state values when commands are mirrored to states. + If ``custom_interface_with_following_offset`` is empty, the offset is applied to the ``position`` state interface. + If a custom interface is set, the ``position`` state value + offset is applied to that interface. Per-Interface Parameters ######################## diff --git a/hardware_interface/doc/semantic_components.rst b/hardware_interface/doc/semantic_components.rst index 1991270170..7beb8b0e59 100644 --- a/hardware_interface/doc/semantic_components.rst +++ b/hardware_interface/doc/semantic_components.rst @@ -12,6 +12,7 @@ List of existing ``SemanticComponentInterface`` `(link to header file) `__, used by :ref:`IMU Sensor Broadcaster ` * `ForceTorqueSensor `__, used by :ref:`Force Torque Sensor Broadcaster ` * `GPSSensor `__ + * `MagneticFieldSensor `__ * `PoseSensor `__, used by :ref:`Pose Broadcaster ` * `RangeSensor `__, used by :ref:`Range Sensor Broadcaster ` diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index f899b60236..63ccfa4190 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -48,6 +48,63 @@ The following is a step-by-step guide to create source files, basic tests, and c In the first line usually the parents ``on_init`` is called to process standard values, like name. This is done using: ``hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)``. If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise. + #. **(Optional) Adding Publisher, Services, etc.** + + A common requirement for a hardware component is to publish status or diagnostic information without interfering with the real-time control loop. + + This allows you to add any standard ROS 2 component (publishers, subscribers, services, timers) to your hardware interface without compromising real-time performance. There are three primary ways to achieve this. + + **Method 1: Using the Framework-Managed Publisher (Recommended & Simplest for HardwareStatus Messages)** + + Refer :ref:`Framework Managed Publisher ` + + **Method 2: Using the Framework-Managed Node (Recommended & Simplest for Custom Messages)** + + The framework internally creates a dedicated ROS 2 node for each hardware component. Your hardware plugin can then get a handle to this node and use it. + + #. **Access and using the Default Node**: You can get a ``shared_ptr`` to the node by calling the ``get_node()`` method and use it just like any other ``rclcpp::Node::SharedPtr`` to create publishers, timers, etc. + + .. code-block:: cpp + + // Continuing inside on_configure() + if (get_node()) + { + my_publisher_ = get_node()->create_publisher("~/status", 10); + + using namespace std::chrono_literals; + my_timer_ = get_node()->create_wall_timer(1s, [this]() { + std_msgs::msg::String msg; + msg.data = "Hardware status update!"; + my_publisher_->publish(msg); + }); + } + + **Method 3: Using the Executor from `HardwareComponentInterfaceParams`** + + For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentInterfaceParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor. + + #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``. + + .. code-block:: cpp + + // In your .hpp + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override; + + #. **Lock and Use the Executor**: Inside ``on_init``, you must safely "lock" the ``weak_ptr`` to get a usable ``shared_ptr``. You can then create your own node and add it to the executor. + + .. code-block:: cpp + + // In your .cpp, inside on_init(params) + if (auto locked_executor = params.executor.lock()) + { + my_custom_node_ = std::make_shared("my_custom_node"); + locked_executor->add_node(my_custom_node_->get_node_base_interface()); + // ... create publishers/timers on my_custom_node_ ... + } + + For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in :ref:`Example 17 `. + #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. @@ -97,6 +154,78 @@ The following is a step-by-step guide to create source files, basic tests, and c #. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. + #. (optional) **Framework Managed Publisher** + + .. _framework_managed_publisher: + + Implement ``init_hardware_status_message`` and ``update_hardware_status_message`` methods to publish the framework-supported hardware status reporting through ``control_msgs/msg/HardwareStatus`` messages: + + * **`init_hardware_status_message`**: This non-realtime method is called once during initialization. You must override it to define the **static structure** of your status message. This includes setting the ``hardware_id``, resizing the ``hardware_device_states`` vector, and for each device, resizing its specific status vectors (e.g., ``generic_hardware_status``, ``canopen_states``) and populating static fields like ``device_id`` and interface ``name``. Pre-allocating the message structure here is crucial for real-time safety. + + .. code-block:: cpp + + // In your .hpp + hardware_interface::CallbackReturn init_hardware_status_message( + control_msgs::msg::HardwareStatus & msg_template) override; + + // In your .cpp + hardware_interface::CallbackReturn MyHardware::init_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) + { + msg.hardware_id = get_hardware_info().name; + msg.hardware_device_states.resize(get_hardware_info().joints.size()); + + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + msg.hardware_device_states[i].device_id = get_hardware_info().joints[i].name; + // This example uses one generic status per joint + msg.hardware_device_states[i].generic_hardware_status.resize(1); + } + return hardware_interface::CallbackReturn::SUCCESS; + } + + * **`update_hardware_status_message`**: This real-time safe method is called from the framework's timer callback. You must override it to **fill in the dynamic values** of the pre-structured message. This typically involves copying your internal state variables (updated in your `read()` method) into the fields of the message. This method must be fast and non-allocating. + + .. code-block:: cpp + + // In your .hpp + hardware_interface::return_type update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) override; + + // In your .cpp + hardware_interface::return_type MyHardware::update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) + { + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + auto & generic_status = msg.hardware_device_states[i].generic_hardware_status; + // Example: Map internal state to a standard status field + if (std::abs(hw_positions_[i]) > joint_limits_[i].max_position) + { + generic_status.health_status = control_msgs::msg::GenericState::HEALTH_ERROR; + } + else + { + generic_status.health_status = control_msgs::msg::GenericState::HEALTH_OK; + } + } + return hardware_interface::return_type::OK; + } + + * **Enable in URDF**: Finally, to activate the publisher, add the ``status_publish_rate`` parameter to your ```` tag in the URDF. Setting it to 0.0 disables the feature. + + .. code-block:: xml + + + + my_package/MyHardware + 20.0 + + ... + + + For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in :ref:`Example 17 `. + #. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header. diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 8d79cfd97b..a9d696a176 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -15,98 +15,11 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_HPP_ -#include -#include -#include - #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/statistics_types.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" +#include "hardware_interface/hardware_component.hpp" namespace hardware_interface { -class ActuatorInterface; - -class Actuator final -{ -public: - Actuator() = default; - - explicit Actuator(std::unique_ptr impl); - - explicit Actuator(Actuator && other) noexcept; - - ~Actuator() = default; - - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); - - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - - const rclcpp_lifecycle::State & configure(); - - const rclcpp_lifecycle::State & cleanup(); - - const rclcpp_lifecycle::State & shutdown(); - - const rclcpp_lifecycle::State & activate(); - - const rclcpp_lifecycle::State & deactivate(); - - const rclcpp_lifecycle::State & error(); - - std::vector export_state_interfaces(); - - std::vector export_command_interfaces(); - - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - const std::string & get_name() const; - - const std::string & get_group_name() const; - - const rclcpp_lifecycle::State & get_lifecycle_state() const; - - const rclcpp::Time & get_last_read_time() const; - - const rclcpp::Time & get_last_write_time() const; - - const HardwareComponentStatisticsCollector & get_read_statistics() const; - - const HardwareComponentStatisticsCollector & get_write_statistics() const; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); - - return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); - - 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_; - // Component statistics - HardwareComponentStatisticsCollector read_statistics_; - HardwareComponentStatisticsCollector write_statistics_; -}; - +using Actuator = HardwareComponent; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__ACTUATOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index e48e1dff17..00941832f8 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,34 +15,10 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/component_parser.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/introspection.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" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#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" +#include "hardware_interface/hardware_component_interface.hpp" namespace hardware_interface { - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /// Virtual Class to implement when integrating a 1 DoF actuator into ros2_control. /** * The typical examples are conveyors or motors. @@ -64,10 +40,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. - * States can be read and command interfaces are available. - * - * As of now, it is left to the hardware component implementation to continue using the command - * received from the ``CommandInterfaces`` or to skip them completely. + * States can be read, but command interfaces are not available. * * FINALIZED (on_shutdown): * Hardware interface is ready for unloading/destruction. @@ -75,7 +48,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * * ACTIVE (on_activate): * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. - * Command interfaces available. + * Command interfaces are available. * * \todo * Implement @@ -88,586 +61,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * HW_IF_EFFORT. */ -class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class ActuatorInterface : public HardwareComponentInterface { public: - ActuatorInterface() - : lifecycle_state_( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), - actuator_logger_(rclcpp::get_logger("actuator_interface")) - { - } - - /// ActuatorInterface copy constructor is actively deleted. - /** - * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid - * failed or simultaneous access to hardware. - */ - ActuatorInterface(const ActuatorInterface & other) = delete; - - ActuatorInterface(ActuatorInterface && other) = delete; - - virtual ~ActuatorInterface() = default; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] logger Logger for the hardware component. - * \param[in] clock_interface pointer to the clock interface. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]] - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) - { - return this->init(hardware_info, logger, clock_interface->get_clock()); - } - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] clock pointer to the resource manager clock. - * \param[in] logger Logger for the hardware component. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - { - actuator_clock_ = clock; - 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) - { - const auto read_start_time = std::chrono::steady_clock::now(); - const auto ret_read = read(time, period); - const auto read_end_time = std::chrono::steady_clock::now(); - read_return_info_.store(ret_read, std::memory_order_release); - read_execution_time_.store( - std::chrono::duration_cast(read_end_time - read_start_time), - std::memory_order_release); - if (ret_read != return_type::OK) - { - return ret_read; - } - const auto write_start_time = std::chrono::steady_clock::now(); - const auto ret_write = write(time, period); - const auto write_end_time = std::chrono::steady_clock::now(); - write_return_info_.store(ret_write, std::memory_order_release); - write_execution_time_.store( - std::chrono::duration_cast(write_end_time - write_start_time), - std::memory_order_release); - return ret_write; - }, - info_.thread_priority); - async_handler_->start_thread(); - } - return on_init(hardware_info); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] hardware_info structure with data from URDF. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) - { - info_ = hardware_info; - parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); - parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); - return CallbackReturn::SUCCESS; - }; - - /// Exports all state interfaces for this hardware interface. - /** - * Old way of exporting the StateInterfaces. If a empty vector is returned then - * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned - * then the exporting of the StateInterfaces is only done with this function and the ownership is - * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not - * be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " - "Exporting is " - "handled " - "by the Framework.")]] virtual std::vector - export_state_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_state_interfaces() and only when empty vector is returned call - // on_export_state_interfaces() - return {}; - } - - /** - * Override this method to export custom StateInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_state_interfaces_ map. - * - * \return vector of descriptions to the unlisted StateInterfaces - */ - virtual std::vector - export_unlisted_state_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the actuator_interface. - * - * \return vector of shared pointers to the created and stored StateInterfaces - */ - virtual std::vector on_export_state_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_state_interface_descriptions(); - - std::vector state_interfaces; - state_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); - - // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_state_interfaces_.insert(std::make_pair(name, description)); - auto state_interface = std::make_shared(description); - actuator_states_.insert(std::make_pair(name, state_interface)); - unlisted_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : joint_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - actuator_states_.insert(std::make_pair(name, state_interface)); - joint_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - return state_interfaces; - } - - /// Exports all command interfaces for this hardware interface. - /** - * Old way of exporting the CommandInterfaces. If a empty vector is returned then - * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is - * returned then the exporting of the CommandInterfaces is only done with this function and the - * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., - * can then not be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_command_interfaces() method. " - "Exporting is " - "handled " - "by the Framework.")]] virtual std::vector - export_command_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_command_interfaces() and only when empty vector is returned call - // on_export_command_interfaces() - return {}; - } - - /** - * Override this method to export custom CommandInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_command_interfaces_ map. - * - * \return vector of descriptions to the unlisted CommandInterfaces - */ - virtual std::vector - export_unlisted_command_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the actuator_interface. - * - * \return vector of shared pointers to the created and stored CommandInterfaces - */ - virtual std::vector on_export_command_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_command_interface_descriptions(); - - std::vector command_interfaces; - command_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); - - // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_command_interfaces_.insert(std::make_pair(name, description)); - auto command_interface = std::make_shared(description); - actuator_commands_.insert(std::make_pair(name, command_interface)); - unlisted_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - for (const auto & [name, descr] : joint_command_interfaces_) - { - auto command_interface = std::make_shared(descr); - actuator_commands_.insert(std::make_pair(name, command_interface)); - joint_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - return command_interfaces; - } - /// Prepare for a new command interface switch. - /** - * Prepare for any mode-switching required by the new command interface combination. - * - * \note This is a non-realtime evaluation of whether a set of command interface claims are - * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be prepared, or - * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. - */ - virtual return_type prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - return return_type::OK; - } - - // Perform switching to the new command interface. - /** - * Perform the mode-switching for the new command interface combination. - * - * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be switched to, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. - */ - virtual return_type perform_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - 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. - */ - HardwareComponentCycleStatus trigger_read( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.result = read_return_info_.load(std::memory_order_acquire); - const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); - if (read_exec_time.count() > 0) - { - status.execution_time = read_exec_time; - } - const auto result = async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - if (!status.successful) - { - RCLCPP_WARN( - get_logger(), - "Trigger read/write called while the previous async trigger is still in progress for " - "hardware interface : '%s'. Failed to trigger read/write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = read(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Read the current state values from the actuator. - /** - * 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. - */ - 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. - */ - HardwareComponentCycleStatus trigger_write( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.successful = true; - const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); - if (write_exec_time.count() > 0) - { - status.execution_time = write_exec_time; - } - status.result = write_return_info_.load(std::memory_order_acquire); - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = write(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Write the current command values to the actuator. - /** - * The physical hardware shall be updated with the latest value from - * the exported command interfaces. - * - * \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. - */ - virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - const std::string & get_name() const { return info_.name; } - - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ - const std::string & get_group_name() const { return info_.group; } - - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ - const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } - - /// Set life-cycle state of the actuator hardware. - /** - * \return state. - */ - void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) - { - lifecycle_state_ = new_state; - } - - template - void set_state(const std::string & interface_name, const T & value) - { - auto it = actuator_states_.find(interface_name); - if (it == actuator_states_.end()) - { - throw std::runtime_error( - "State interface not found: " + interface_name + - " in actuator hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_state(const std::string & interface_name) const - { - auto it = actuator_states_.find(interface_name); - if (it == actuator_states_.end()) - { - throw std::runtime_error( - "State interface not found: " + interface_name + - " in actuator hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - "Failed to get state value from interface: " + interface_name + - ". This should not happen."); - } - return opt_value.value(); - } - - void set_command(const std::string & interface_name, const double & value) - { - auto it = actuator_commands_.find(interface_name); - if (it == actuator_commands_.end()) - { - throw std::runtime_error( - "Command interface not found: " + interface_name + - " in actuator hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_command(const std::string & interface_name) const - { - auto it = actuator_commands_.find(interface_name); - if (it == actuator_commands_.end()) - { - throw std::runtime_error( - "Command interface not found: " + interface_name + - " in actuator hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - "Failed to get command value from interface: " + interface_name + - ". This should not happen."); - } - return opt_value.value(); - } - - /// Get the logger of the ActuatorInterface. - /** - * \return logger of the ActuatorInterface. - */ - rclcpp::Logger get_logger() const { return actuator_logger_; } - - /// Get the clock of the ActuatorInterface. - /** - * \return clock of the ActuatorInterface. - */ - rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; } - - /// Get the hardware info of the ActuatorInterface. - /** - * \return hardware info of the ActuatorInterface. - */ - const HardwareInfo & get_hardware_info() const { return info_; } - - /// Prepare for the activation of the hardware. - /** - * This method is called before the hardware is activated by the resource manager. - */ - void prepare_for_activation() - { - read_return_info_.store(return_type::OK, std::memory_order_release); - read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - write_return_info_.store(return_type::OK, std::memory_order_release); - write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - } - - /// Enable or disable introspection of the hardware. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ - void enable_introspection(bool enable) - { - if (enable) - { - stats_registrations_.enableAll(); - } - else - { - stats_registrations_.disableAll(); - } - } - -protected: - HardwareInfo info_; - // interface names to InterfaceDescription - std::unordered_map joint_state_interfaces_; - std::unordered_map joint_command_interfaces_; - - std::unordered_map unlisted_state_interfaces_; - std::unordered_map unlisted_command_interfaces_; - - // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector joint_states_; - std::vector joint_commands_; - - std::vector unlisted_states_; - std::vector unlisted_commands_; - - rclcpp_lifecycle::State lifecycle_state_; - std::unique_ptr> async_handler_; - -private: - rclcpp::Clock::SharedPtr actuator_clock_; - rclcpp::Logger actuator_logger_; - // interface names to Handle accessed through getters/setters - std::unordered_map actuator_states_; - std::unordered_map actuator_commands_; - std::atomic read_return_info_ = return_type::OK; - std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); - std::atomic write_return_info_ = return_type::OK; - std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); - -protected: - pal_statistics::RegistrationsRAII stats_registrations_; + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index cb61ed83c5..3e1e306db6 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,6 +15,8 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include + #include #include #include @@ -23,16 +25,21 @@ #include #include #include +#include #include #include #include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/introspection.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/macros.hpp" +#include "rclcpp/logging.hpp" + namespace { +#ifndef _WIN32 template std::string get_type_name() { @@ -41,6 +48,14 @@ std::string get_type_name() abi::__cxa_demangle(typeid(T).name(), nullptr, nullptr, &status), std::free}; return (status == 0) ? res.get() : typeid(T).name(); } +#else +// not supported on Windows, use typeid directly +template +std::string get_type_name() +{ + return typeid(T).name(); +} +#endif } // namespace namespace hardware_interface @@ -53,10 +68,7 @@ class Handle { public: [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - Handle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) + Handle(const std::string & prefix_name, const std::string & interface_name, double * value_ptr) : prefix_name_(prefix_name), interface_name_(interface_name), handle_name_(prefix_name_ + "/" + interface_name_), @@ -64,32 +76,55 @@ class Handle { } - explicit Handle(const InterfaceDescription & interface_description) - : prefix_name_(interface_description.get_prefix_name()), - interface_name_(interface_description.get_interface_name()), - handle_name_(interface_description.get_name()) + explicit Handle( + const std::string & prefix_name, const std::string & interface_name, + const std::string & data_type = "double", const std::string & initial_value = "") + : prefix_name_(prefix_name), + interface_name_(interface_name), + handle_name_(prefix_name_ + "/" + interface_name_), + data_type_(data_type) { - data_type_ = interface_description.get_data_type(); // As soon as multiple datatypes are used in HANDLE_DATATYPE // we need to initialize according the type passed in interface description if (data_type_ == hardware_interface::HandleDataType::DOUBLE) { - value_ = std::numeric_limits::quiet_NaN(); - value_ptr_ = std::get_if(&value_); + try + { + value_ = initial_value.empty() ? std::numeric_limits::quiet_NaN() + : hardware_interface::stod(initial_value); + value_ptr_ = std::get_if(&value_); + } + catch (const std::invalid_argument & err) + { + throw std::invalid_argument( + fmt::format( + FMT_COMPILE("Invalid initial value: '{}' parsed for interface: '{}' with type: '{}'"), + initial_value, handle_name_, data_type_.to_string())); + } } else if (data_type_ == hardware_interface::HandleDataType::BOOL) { value_ptr_ = nullptr; - value_ = false; + value_ = initial_value.empty() ? false : hardware_interface::parse_bool(initial_value); } else { throw std::runtime_error( - "Invalid data type : '" + interface_description.interface_info.data_type + - "' for interface : " + interface_description.get_name()); + fmt::format( + FMT_COMPILE( + "Invalid data type: '{}' for interface: {}. Supported types are double and bool."), + data_type, handle_name_)); } } + explicit Handle(const InterfaceDescription & interface_description) + : Handle( + interface_description.get_prefix_name(), interface_description.get_interface_name(), + interface_description.get_data_type_string(), + interface_description.interface_info.initial_value) + { + } + [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const std::string & interface_name) @@ -132,32 +167,8 @@ class Handle const std::string & get_interface_name() const { return interface_name_; } - [[deprecated( - "Replaced by get_name method, which is semantically more correct")]] const std::string & - get_full_name() const - { - return get_name(); - } - const std::string & get_prefix_name() const { return prefix_name_; } - [[deprecated( - "Use std::optional get_optional() instead to retrieve the value. This method will be " - "removed by the ROS 2 Kilted Kaiju release.")]] - double get_value() const - { - std::shared_lock lock(handle_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - return std::numeric_limits::quiet_NaN(); - } - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) return value_ if old functionality is removed - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - // END - } - /** * @brief Get the value of the handle. * @tparam T The type of the value to be retrieved. @@ -196,9 +207,30 @@ class Handle // TODO(saikishor) return value_ if old functionality is removed if constexpr (std::is_same_v) { - // If the template is of type double, check if the value_ptr_ is not nullptr - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + switch (data_type_) + { + case HandleDataType::DOUBLE: + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + case HandleDataType::BOOL: + // TODO(christophfroehlich): replace with RCLCPP_WARN_ONCE once + // https://github.com/ros2/rclcpp/issues/2587 + // is fixed + if (!notified_) + { + RCLCPP_WARN( + rclcpp::get_logger(get_name()), + "Casting bool to double for interface: %s. Better use get_optional().", + get_name().c_str()); + notified_ = true; + } + return static_cast(std::get(value_)); + default: + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Data type: '{}' cannot be casted to double for interface: {}"), + data_type_.to_string(), get_name())); + } } try { @@ -207,59 +239,13 @@ class Handle catch (const std::bad_variant_access & err) { throw std::runtime_error( - "Invalid data type : '" + get_type_name() + "' access for interface : " + get_name() + - " expected : '" + data_type_.to_string() + "'"); + fmt::format( + FMT_COMPILE("Invalid data type: '{}' access for interface: {} expected: '{}'"), + get_type_name(), get_name(), data_type_.to_string())); } // END } - /** - * @brief Get the value of the handle. - * @tparam T The type of the value to be retrieved. - * @param value The value of the handle. - * @return true if the value is accessed successfully, false otherwise. - * - * @note The method is thread-safe and non-blocking. - * @note When different threads access the same handle at same instance, and if they are unable to - * lock the handle to access the value, the handle returns false. If the operation is successful, - * the value is updated and returns true. - */ - template - [[deprecated( - "Use std::optional get_optional() instead to retrieve the value. This method will be " - "removed by the ROS 2 Kilted Kaiju release.")]] [[nodiscard]] bool - get_value(T & value) const - { - std::shared_lock lock(handle_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - return false; - } - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) return value_ if old functionality is removed - if constexpr (std::is_same_v) - { - // If the template is of type double, check if the value_ptr_ is not nullptr - THROW_ON_NULLPTR(value_ptr_); - value = *value_ptr_; - } - else - { - try - { - value = std::get(value_); - } - catch (const std::bad_variant_access & err) - { - throw std::runtime_error( - "Invalid data type : '" + get_type_name() + "' access for interface : " + get_name() + - " expected : '" + data_type_.to_string() + "'"); - } - } - return true; - // END - } - /** * @brief Set the value of the handle. * @tparam T The type of the value to be set. @@ -310,8 +296,9 @@ class Handle if (!std::holds_alternative(value_)) { throw std::runtime_error( - "Invalid data type : '" + get_type_name() + "' access for interface : " + get_name() + - " expected : '" + data_type_.to_string() + "'"); + fmt::format( + FMT_COMPILE("Invalid data type: '{}' access for interface: {} expected: '{}'"), + get_type_name(), get_name(), data_type_.to_string())); } value_ = value; } @@ -323,6 +310,9 @@ class Handle HandleDataType get_data_type() const { return data_type_; } + /// Returns true if the handle data type can be casted to double. + bool is_castable_to_double() const { return data_type_.is_castable_to_double(); } + private: void copy(const Handle & other) noexcept { @@ -362,6 +352,12 @@ class Handle double * value_ptr_; // END mutable std::shared_mutex handle_mutex_; + +private: + // TODO(christophfroehlich): remove once + // https://github.com/ros2/rclcpp/issues/2587 + // is fixed + mutable bool notified_ = false; }; class StateInterface : public Handle @@ -394,7 +390,11 @@ class StateInterface : public Handle StateInterface(StateInterface && other) = default; +// Disable deprecated warnings +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" using Handle::Handle; +#pragma GCC diagnostic pop using SharedPtr = std::shared_ptr; using ConstSharedPtr = std::shared_ptr; @@ -464,7 +464,11 @@ class CommandInterface : public Handle } } +// Disable deprecated warnings +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" using Handle::Handle; +#pragma GCC diagnostic pop using SharedPtr = std::shared_ptr; diff --git a/hardware_interface/include/hardware_interface/hardware_component.hpp b/hardware_interface/include/hardware_interface/hardware_component.hpp new file mode 100644 index 0000000000..c7b2cd2e98 --- /dev/null +++ b/hardware_interface/include/hardware_interface/hardware_component.hpp @@ -0,0 +1,114 @@ +// Copyright 2020 - 2021 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__HARDWARE_COMPONENT_HPP_ +#define HARDWARE_INTERFACE__HARDWARE_COMPONENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_component_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace hardware_interface +{ +class HardwareComponentInterface; + +class HardwareComponent final +{ +public: + HardwareComponent() = default; + + explicit HardwareComponent(std::unique_ptr impl); + + explicit HardwareComponent(HardwareComponent && other) noexcept; + + ~HardwareComponent() = default; + + HardwareComponent(const HardwareComponent & other) = delete; + + HardwareComponent & operator=(const HardwareComponent & other) = delete; + + HardwareComponent & operator=(HardwareComponent && other) = delete; + + const rclcpp_lifecycle::State & initialize( + const hardware_interface::HardwareComponentParams & params); + + const rclcpp_lifecycle::State & configure(); + + const rclcpp_lifecycle::State & cleanup(); + + const rclcpp_lifecycle::State & shutdown(); + + const rclcpp_lifecycle::State & activate(); + + const rclcpp_lifecycle::State & deactivate(); + + const rclcpp_lifecycle::State & error(); + + std::vector export_state_interfaces(); + + std::vector export_command_interfaces(); + + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces); + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces); + + const std::string & get_name() const; + + const std::string & get_group_name() const; + + const rclcpp_lifecycle::State & get_lifecycle_state() const; + + const rclcpp::Time & get_last_read_time() const; + + const rclcpp::Time & get_last_write_time() const; + + const HardwareComponentStatisticsCollector & get_read_statistics() const; + + const HardwareComponentStatisticsCollector & get_write_statistics() const; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); + + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + + std::recursive_mutex & get_mutex(); + +private: + std::unique_ptr impl_; + mutable std::recursive_mutex component_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; + HardwareComponentStatisticsCollector write_statistics_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_COMPONENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp new file mode 100644 index 0000000000..3d1cf89ccd --- /dev/null +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -0,0 +1,952 @@ +// Copyright 2025 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__HARDWARE_COMPONENT_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INTERFACE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/msg/hardware_status.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/introspection.hpp" +#include "hardware_interface/types/hardware_component_interface_params.hpp" +#include "hardware_interface/types/hardware_component_params.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "hardware_interface/types/trigger_type.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/version.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" + +namespace hardware_interface +{ + +static inline rclcpp::NodeOptions get_hardware_component_node_options() +{ + rclcpp::NodeOptions node_options; +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 21 + node_options.enable_logger_service(true); +#endif + return node_options; +} + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +/** + * @brief Virtual base class for all hardware components (Actuators, Sensors, and Systems). + * + * This class provides the common structure and functionality for all hardware components, + * including lifecycle management, interface handling, and asynchronous support. Hardware + * plugins should inherit from one of its derivatives: ActuatorInterface, SensorInterface, + * or SystemInterface. + */ +class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +{ +public: + HardwareComponentInterface() + : lifecycle_state_( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), + logger_(rclcpp::get_logger("hardware_component_interface")) + { + } + + /// HardwareComponentInterface copy constructor is actively deleted. + /** + * Hardware interfaces have unique ownership and thus can't be copied in order to avoid + * failed or simultaneous access to hardware. + */ + HardwareComponentInterface(const HardwareComponentInterface & other) = delete; + + HardwareComponentInterface(HardwareComponentInterface && other) = delete; + + virtual ~HardwareComponentInterface() = default; + + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] params A struct of type HardwareComponentParams containing all necessary + * parameters for initializing this specific hardware component, + * including its HardwareInfo, a dedicated logger, a clock, and a + * weak_ptr to the executor. + * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks + * such as `spin()`. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init(const hardware_interface::HardwareComponentParams & params) + { + clock_ = params.clock; + auto logger_copy = params.logger; + logger_ = logger_copy.get_child( + "hardware_component." + params.hardware_info.type + "." + params.hardware_info.name); + info_ = params.hardware_info; + if (params.hardware_info.is_async) + { + realtime_tools::AsyncFunctionHandlerParams async_thread_params; + async_thread_params.thread_priority = info_.async_params.thread_priority; + async_thread_params.scheduling_policy = + realtime_tools::AsyncSchedulingPolicy(info_.async_params.scheduling_policy); + async_thread_params.cpu_affinity_cores = info_.async_params.cpu_affinity_cores; + async_thread_params.clock = params.clock; + async_thread_params.logger = params.logger; + async_thread_params.exec_rate = params.hardware_info.rw_rate; + async_thread_params.print_warnings = info_.async_params.print_warnings; + RCLCPP_INFO( + get_logger(), "Starting async handler with scheduler priority: %d and policy : %s", + info_.async_params.thread_priority, + async_thread_params.scheduling_policy.to_string().c_str()); + async_handler_ = std::make_unique>(); + const bool is_sensor_type = (info_.type == "sensor"); + async_handler_->init( + [this, is_sensor_type](const rclcpp::Time & time, const rclcpp::Duration & period) + { + const auto read_start_time = std::chrono::steady_clock::now(); + const auto ret_read = read(time, period); + const auto read_end_time = std::chrono::steady_clock::now(); + read_return_info_.store(ret_read, std::memory_order_release); + read_execution_time_.store( + std::chrono::duration_cast(read_end_time - read_start_time), + std::memory_order_release); + if (ret_read != return_type::OK) + { + return ret_read; + } + if ( + !is_sensor_type && + this->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + const auto write_start_time = std::chrono::steady_clock::now(); + const auto ret_write = write(time, period); + const auto write_end_time = std::chrono::steady_clock::now(); + write_return_info_.store(ret_write, std::memory_order_release); + write_execution_time_.store( + std::chrono::duration_cast( + write_end_time - write_start_time), + std::memory_order_release); + return ret_write; + } + return return_type::OK; + }, + async_thread_params); + async_handler_->start_thread(); + } + + if (auto locked_executor = params.executor.lock()) + { + std::string node_name = hardware_interface::to_lower_case(params.hardware_info.name); + std::replace(node_name.begin(), node_name.end(), '/', '_'); + hardware_component_node_ = std::make_shared( + node_name, params.node_namespace, get_hardware_component_node_options()); + locked_executor->add_node(hardware_component_node_->get_node_base_interface()); + } + else + { + RCLCPP_WARN( + params.logger, + "Executor is not available during hardware component initialization for '%s'. Skipping " + "node creation!", + params.hardware_info.name.c_str()); + } + + double publish_rate = 0.0; + auto it = info_.hardware_parameters.find("status_publish_rate"); + if (it != info_.hardware_parameters.end()) + { + try + { + publish_rate = hardware_interface::stod(it->second); + } + catch (const std::invalid_argument &) + { + RCLCPP_WARN( + get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.", + publish_rate); + } + } + + if (publish_rate == 0.0) + { + RCLCPP_INFO( + get_logger(), + "`status_publish_rate` is set to 0.0, hardware status publisher will not be created."); + } + else + { + control_msgs::msg::HardwareStatus status_msg_template; + if (init_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) + { + RCLCPP_ERROR(get_logger(), "User-defined 'init_hardware_status_message' failed."); + return CallbackReturn::ERROR; + } + + if (!status_msg_template.hardware_device_states.empty()) + { + if (!hardware_component_node_) + { + RCLCPP_WARN( + get_logger(), + "Hardware status message was configured, but no node is available for the publisher. " + "Publisher will not be created."); + } + else + { + try + { + hardware_status_publisher_ = + hardware_component_node_->create_publisher( + "~/hardware_status", rclcpp::SystemDefaultsQoS()); + + hardware_status_timer_ = hardware_component_node_->create_wall_timer( + std::chrono::duration(1.0 / publish_rate), + [this]() + { + std::optional msg_to_publish_opt; + hardware_status_box_.get(msg_to_publish_opt); + + if (msg_to_publish_opt.has_value() && hardware_status_publisher_) + { + control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value(); + if (update_hardware_status_message(msg) != return_type::OK) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *clock_, 1000, + "User's update_hardware_status_message() failed for '%s'.", + info_.name.c_str()); + return; + } + msg.header.stamp = this->get_clock()->now(); + hardware_status_publisher_->publish(msg); + } + }); + hardware_status_box_.set(std::make_optional(status_msg_template)); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Exception during publisher/timer setup for hardware status: %s", + e.what()); + return CallbackReturn::ERROR; + } + } + } + else + { + RCLCPP_WARN( + get_logger(), + "`status_publish_rate` was set to a non-zero value, but no hardware status message was " + "configured. Publisher will not be created. Are you sure " + "init_hardware_status_message() is set up properly?"); + } + } + + hardware_interface::HardwareComponentInterfaceParams interface_params; + interface_params.hardware_info = info_; + interface_params.executor = params.executor; + return on_init(interface_params); + }; + + /// User-overridable method to configure the structure of the HardwareStatus message. + /** + * To enable status publishing, override this method to pre-allocate the message structure + * and fill in static information like device IDs and interface names. This method is called + * once during the non-realtime `init()` phase. If the `hardware_device_states` vector is + * left empty, publishing will be disabled. + * + * \param[out] msg_template A reference to a HardwareStatus message to be configured. + * \returns CallbackReturn::SUCCESS if configured successfully, CallbackReturn::ERROR on failure. + */ + virtual CallbackReturn init_hardware_status_message( + control_msgs::msg::HardwareStatus & /*msg_template*/) + { + // Default implementation does nothing, disabling the feature. + return CallbackReturn::SUCCESS; + } + + /// User-overridable method to fill the hardware status message with real-time data. + /** + * This real-time safe method is called by the framework within the `trigger_read()` loop. + * Override this method to populate the `value` fields of the pre-allocated message with the + * latest hardware states that were updated in your `read()` method. + * + * \param[in,out] msg The pre-allocated message to be filled with the latest values. + * \returns return_type::OK on success, return_type::ERROR on failure. + */ + virtual return_type update_hardware_status_message(control_msgs::msg::HardwareStatus & /*msg*/) + { + // Default implementation does nothing. + return return_type::OK; + } + + /// Initialization of the hardware interface from data parsed from the robot's URDF. + /** + * \param[in] params A struct of type hardware_interface::HardwareComponentInterfaceParams + * containing all necessary parameters for initializing this specific hardware component, + * specifically its HardwareInfo, and a weak_ptr to the executor. + * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks + * such as `spin()`. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + virtual CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) + { + info_ = params.hardware_info; + if (info_.type == "actuator") + { + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + } + else if (info_.type == "sensor") + { + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + } + else if (info_.type == "system") + { + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); + } + return CallbackReturn::SUCCESS; + }; + + /// Exports all state interfaces for this hardware interface. + /** + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. + * + * Note the ownership over the state interfaces is transferred to the caller. + * + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_unlisted_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_state_interface_descriptions(); + + std::vector state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + hardware_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + hardware_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + for (const auto & [name, descr] : sensor_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + hardware_states_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + for (const auto & [name, descr] : gpio_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + hardware_states_.insert(std::make_pair(name, state_interface)); + gpio_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + return state_interfaces; + } + + /// Exports all command interfaces for this hardware interface. + /** + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. + * + * Note the ownership over the state interfaces is transferred to the caller. + * + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector on_export_command_interfaces() method. " + "Exporting is handled by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_command_interfaces() and only when empty vector is returned call + // on_export_command_interfaces() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector + export_unlisted_command_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * Actuator and System components should override this method. Sensor components can use the + * default. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + virtual std::vector on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_command_interface_descriptions(); + + std::vector command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + + gpio_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + hardware_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + hardware_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + hardware_commands_.insert(std::make_pair(name, command_interface)); + gpio_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + return command_interfaces; + } + + /// Prepare for a new command interface switch. + /** + * Prepare for any mode-switching required by the new command interface combination. + * + * \note This is a non-realtime evaluation of whether a set of command interface claims are + * possible, and call to start preparing data structures for the upcoming switch that will occur. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be prepared (or) if the + * interface key is not relevant to this system. Returns return_type::ERROR otherwise. + */ + virtual return_type prepare_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) + { + return return_type::OK; + } + + // Perform switching to the new command interface. + /** + * Perform the mode-switching for the new command interface combination. + * + * \note This is part of the realtime update loop, and should be fast. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be switched to (or) if the + * interface key is not relevant to this system. Returns return_type::ERROR otherwise. + */ + virtual return_type perform_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) + { + 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. + */ + HardwareComponentCycleStatus trigger_read( + const rclcpp::Time & time, const rclcpp::Duration & period) + { + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; + if (info_.is_async) + { + status.result = read_return_info_.load(std::memory_order_acquire); + const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); + if (read_exec_time.count() > 0) + { + status.execution_time = read_exec_time; + } + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + if (!status.successful) + { + RCLCPP_WARN_EXPRESSION( + get_logger(), info_.async_params.print_warnings, + "Trigger read/write called while the previous async trigger is still in progress for " + "hardware interface : '%s'. Failed to trigger read/write cycle!", + info_.name.c_str()); + status.result = return_type::OK; + return status; + } + } + else + { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = read(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + } + return status; + } + + /// Read the current state values from the hardware. + /** + * 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. + */ + 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. + */ + HardwareComponentCycleStatus trigger_write( + const rclcpp::Time & time, const rclcpp::Duration & period) + { + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; + if (info_.is_async) + { + status.successful = true; + const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); + if (write_exec_time.count() > 0) + { + status.execution_time = write_exec_time; + } + status.result = write_return_info_.load(std::memory_order_acquire); + } + else + { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = write(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + } + return status; + } + + /// Write the current command values to the hardware. + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * + * \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. + */ + virtual return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + { + return return_type::OK; + } + + /// Get name of the hardware. + /** + * \return name. + */ + const std::string & get_name() const { return info_.name; } + + /// Get name of the hardware group to which it belongs to. + /** + * \return group name. + */ + const std::string & get_group_name() const { return info_.group; } + + /// Get life-cycle state of the hardware. + /** + * \return state. + */ + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } + + /// Set life-cycle state of the hardware. + /** + * \return state. + */ + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } + + /// Does the state interface exist? + /** + * \param[in] interface_name The name of the state interface. + * \return true if the state interface exists, false otherwise. + */ + bool has_state(const std::string & interface_name) const + { + return hardware_states_.find(interface_name) != hardware_states_.end(); + } + + /// Set the value of a state interface. + /** + * \tparam T The type of the value to be stored. + * \param[in] interface_name The name of the state interface to access. + * \param[in] value The value to store. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the state interface. + */ + template + void set_state(const std::string & interface_name, const T & value) + { + auto it = hardware_states_.find(interface_name); + if (it == hardware_states_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "State interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::unique_lock lock(handle->get_mutex()); + std::ignore = handle->set_value(lock, value); + } + + /// Get the value from a state interface. + /** + * \tparam T The type of the value to be retrieved. + * \param[in] interface_name The name of the state interface to access. + * \return The value obtained from the interface. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the state interface or its stored value. + */ + template + T get_state(const std::string & interface_name) const + { + auto it = hardware_states_.find(interface_name); + if (it == hardware_states_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "State interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::shared_lock lock(handle->get_mutex()); + const auto opt_value = handle->get_optional(lock); + if (!opt_value) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Failed to get state value from interface: {}. This should not happen."), + interface_name)); + } + return opt_value.value(); + } + + /// Does the command interface exist? + /** + * \param[in] interface_name The name of the command interface. + * \return true if the command interface exists, false otherwise. + */ + bool has_command(const std::string & interface_name) const + { + return hardware_commands_.find(interface_name) != hardware_commands_.end(); + } + + /// Set the value of a command interface. + /** + * \tparam T The type of the value to be stored. + * \param interface_name The name of the command + * interface to access. + * \param value The value to store. + * \throws This method throws a runtime error if it + * cannot access the command interface. + */ + template + void set_command(const std::string & interface_name, const T & value) + { + auto it = hardware_commands_.find(interface_name); + if (it == hardware_commands_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "Command interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::unique_lock lock(handle->get_mutex()); + std::ignore = handle->set_value(lock, value); + } + + /// Get the value from a command interface. + /** + * \tparam T The type of the value to be retrieved. + * \param[in] interface_name The name of the command interface to access. + * \return The value obtained from the interface. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the command interface or its stored value. + */ + template + T get_command(const std::string & interface_name) const + { + auto it = hardware_commands_.find(interface_name); + if (it == hardware_commands_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "Command interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::shared_lock lock(handle->get_mutex()); + const auto opt_value = handle->get_optional(lock); + if (!opt_value) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Failed to get command value from interface: {}. This should not happen."), + interface_name)); + } + return opt_value.value(); + } + + /// Get the logger of the HardwareComponentInterface. + /** + * \return logger of the HardwareComponentInterface. + */ + rclcpp::Logger get_logger() const { return logger_; } + + /// Get the clock + /** + * \return clock that is shared with the controller manager + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + + /// Get the default node of the HardwareComponentInterface. + /** + * \return node of the HardwareComponentInterface. + */ + rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } + + /// Get the hardware info of the HardwareComponentInterface. + /** + * \return hardware info of the HardwareComponentInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + + /// Pause any asynchronous operations. + /** + * This method is called to pause any ongoing asynchronous operations, such as read/write cycles. + * It is typically used during lifecycle transitions or when the hardware needs to be paused. + */ + void pause_async_operations() + { + if (async_handler_) + { + async_handler_->pause_execution(); + } + } + + /// Prepare for the activation of the hardware. + /** + * This method is called before the hardware is activated by the resource manager. + */ + void prepare_for_activation() + { + read_return_info_.store(return_type::OK, std::memory_order_release); + read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + write_return_info_.store(return_type::OK, std::memory_order_release); + write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + } + + /// Enable or disable introspection of the hardware. + /** + * \param[in] enable Enable introspection if true, disable otherwise. + */ + void enable_introspection(bool enable) + { + if (enable) + { + stats_registrations_.enableAll(); + } + else + { + stats_registrations_.disableAll(); + } + } + +protected: + HardwareInfo info_; + // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; + + std::unordered_map sensor_state_interfaces_; + + std::unordered_map gpio_state_interfaces_; + std::unordered_map gpio_command_interfaces_; + + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; + + rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; + + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; + std::vector joint_commands_; + + std::vector sensor_states_; + + std::vector gpio_states_; + std::vector gpio_commands_; + + std::vector unlisted_states_; + std::vector unlisted_commands_; + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; + // interface names to Handle accessed through getters/setters + std::unordered_map hardware_states_; + std::unordered_map hardware_commands_; + std::atomic read_return_info_ = return_type::OK; + std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); + std::atomic write_return_info_ = return_type::OK; + std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; + std::shared_ptr> hardware_status_publisher_; + realtime_tools::RealtimeThreadSafeBox> + hardware_status_box_; + rclcpp::TimerBase::SharedPtr hardware_status_timer_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_COMPONENT_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index e569b16433..27d3d13ae3 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -35,11 +35,11 @@ struct InterfaceInfo */ std::string name; /// (Optional) Minimal allowed values of the interface. - std::string min; + std::string min = ""; /// (Optional) Maximal allowed values of the interface. - std::string max; + std::string max = ""; /// (Optional) Initial value of the interface. - std::string initial_value; + std::string initial_value = ""; /// (Optional) The datatype of the interface, e.g. "bool", "int". std::string data_type = "double"; /// (Optional) If the handle is an array, the size of the array. @@ -184,6 +184,24 @@ class HandleDataType } } + /** + * @brief Check if the HandleDataType can be casted to double. + * @return True if the HandleDataType can be casted to double, false otherwise. + * @note Once we add support for more data types, this function should be updated + */ + bool is_castable_to_double() const + { + switch (value_) + { + case DOUBLE: + return true; + case BOOL: + return true; // bool can be converted to double + default: + return false; // unknown type cannot be converted + } + } + HandleDataType from_string(const std::string & data_type) { return HandleDataType(data_type); } private: @@ -224,10 +242,31 @@ struct InterfaceDescription const std::string & get_name() const { return interface_name; } + const std::string & get_data_type_string() const { return interface_info.data_type; } + HandleDataType get_data_type() const { return HandleDataType(interface_info.data_type); } }; +struct HardwareAsyncParams +{ + /// Thread priority for the async worker thread + int thread_priority = 50; + /// Scheduling policy for the async worker thread + std::string scheduling_policy = "synchronized"; + /// CPU affinity cores for the async worker thread + std::vector cpu_affinity_cores = {}; + /// Whether to print warnings when the async thread doesn't meet its deadline + bool print_warnings = true; +}; + /// This structure stores information about hardware defined in a robot's URDF. +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif struct HardwareInfo { /// Name of the hardware. @@ -241,7 +280,9 @@ struct HardwareInfo /// Component is async bool is_async; /// Async thread priority - int thread_priority; + [[deprecated("Use async_params instead.")]] int thread_priority; + /// Async Parameters + HardwareAsyncParams async_params; /// Name of the pluginlib plugin of the hardware that will be loaded. std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. @@ -286,6 +327,11 @@ struct HardwareInfo */ std::unordered_map soft_limits; }; +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index 605ecb9216..85d51fefa4 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -38,7 +38,7 @@ template { if constexpr (std::is_same_v>) { - return std::find(container.begin(), container.end(), item); + return std::find(container.cbegin(), container.cend(), item); } else if constexpr ( std::is_same_v> || @@ -68,7 +68,7 @@ template template [[nodiscard]] bool has_item(const Container & container, const T & item) { - return get_item_iterator(container, item) != container.end(); + return get_item_iterator(container, item) != container.cend(); } /** @@ -85,6 +85,20 @@ void add_item(std::vector & vector, const T & item) } } +/** + * @brief Add the items from one container to the other, if they are not already in it. + * @param vector The container to add the items to. + * @param items The container to add the items from. + */ +template +void add_items(std::vector & vector, const std::vector & items) +{ + for (const auto & item : items) + { + add_item(vector, item); + } +} + /** * @brief Remove the item from the container if it is in it. * @param container The container to remove the item from. diff --git a/hardware_interface/include/hardware_interface/introspection.hpp b/hardware_interface/include/hardware_interface/introspection.hpp index d25b71c2d8..dff4ee6b13 100644 --- a/hardware_interface/include/hardware_interface/introspection.hpp +++ b/hardware_interface/include/hardware_interface/introspection.hpp @@ -17,13 +17,37 @@ #ifndef HARDWARE_INTERFACE__INTROSPECTION_HPP_ #define HARDWARE_INTERFACE__INTROSPECTION_HPP_ +#include + +#include "hardware_interface/types/statistics_types.hpp" #include "pal_statistics/pal_statistics_macros.hpp" #include "pal_statistics/pal_statistics_utils.hpp" +namespace pal_statistics +{ +template <> +inline IdType customRegister( + StatisticsRegistry & registry, const std::string & name, + const libstatistics_collector::moving_average_statistics::StatisticData * variable, + RegistrationsRAII * bookkeeping, bool enabled) +{ + registry.registerVariable(name + "/max", &variable->max, bookkeeping, enabled); + registry.registerVariable(name + "/min", &variable->min, bookkeeping, enabled); + registry.registerVariable(name + "/average", &variable->average, bookkeeping, enabled); + registry.registerVariable( + name + "/standard_deviation", &variable->standard_deviation, bookkeeping, enabled); + std::function sample_func = [variable] + { return static_cast(variable->sample_count); }; + return registry.registerFunction(name + "/sample_count", sample_func, bookkeeping, enabled); +} +} // namespace pal_statistics + namespace hardware_interface { constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control"; constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "~/introspection_data"; +constexpr char CM_STATISTICS_KEY[] = "cm_execution_statistics"; +constexpr char CM_STATISTICS_TOPIC[] = "~/statistics"; #define REGISTER_ROS2_CONTROL_INTROSPECTION(ID, ENTITY) \ REGISTER_ENTITY( \ diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp index ba9fd6c4ab..fa6c9c487f 100644 --- a/hardware_interface/include/hardware_interface/lexical_casts.hpp +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -15,7 +15,12 @@ #ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ #define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ +#include +#include +#include #include +#include +#include namespace hardware_interface { @@ -27,8 +32,94 @@ namespace hardware_interface */ double stod(const std::string & s); +/** + * \brief Convert a string to lower case. + * \param string The input string. + * \return The lower case version of the input string. + */ +std::string to_lower_case(const std::string & string); + +/** + * \brief Parse a boolean value from a string. + * \param bool_string The input string, can be "true", "false" (case insensitive) + * \return The parsed boolean value. + * \throws std::invalid_argument if the string is not a valid boolean representation. + */ bool parse_bool(const std::string & bool_string); +template +std::vector parse_array(const std::string & array_string) +{ + // Use regex to check for a flat array: starts with [, ends with ], no nested brackets + const std::regex array_regex(R"(^\[\s*([^\[\]]*\s*(,\s*[^\[\]]+\s*)*)?\]$)"); + if (!std::regex_match(array_string, array_regex)) + { + throw std::invalid_argument( + "String must be a flat array: starts with '[' and ends with ']', no nested arrays"); + } + + // Use regex for the expression that either empty or contains only spaces + const std::regex empty_or_spaces_regex(R"(^\[\s*\]$)"); + if (std::regex_match(array_string, empty_or_spaces_regex)) + { + return {}; // Return empty array if input is "[]" + } + + // Use regex to find cases of comma-separated but only whitespaces or no spaces between them like + // "[,]" "[a,b,,c]" + const std::regex comma_separated_regex(R"(^\[\s*([^,\s]+(\s*,\s*[^,\s]+)*)?\s*\]$)"); + if (!std::regex_match(array_string, comma_separated_regex)) + { + throw std::invalid_argument( + "String must be a flat array with comma-separated values and no spaces between them"); + } + + std::vector result = {}; + if (array_string == "[]") + { + return result; // Return empty array if input is "[]" + } + + // regex for comma separated values and no spaces between them or just content like "[a,b,c]" or + // "[a]" or "[a, b, c]" + const std::regex value_regex(R"([^\s,\[\]]+)"); + auto begin = std::sregex_iterator(array_string.begin(), array_string.end(), value_regex); + auto end = std::sregex_iterator(); + + for (auto it = begin; it != end; ++it) + { + const std::string value_str = it->str(); // Get the first capturing group + if constexpr (std::is_same_v) + { + result.push_back(value_str); + } + else if constexpr (std::is_same_v) + { + result.push_back(parse_bool(value_str)); + } + else if constexpr (std::is_floating_point_v || std::is_integral_v) + { + try + { + const T value = static_cast(hardware_interface::stod(value_str)); + result.push_back(value); + } + catch (const std::exception &) + { + throw std::invalid_argument( + "Failed converting string to floating point or integer: " + value_str); + } + } + else + { + throw std::invalid_argument("Unsupported type for parsing: " + std::string(typeid(T).name())); + } + } + return result; +} + +std::vector parse_string_array(const std::string & string_array_string); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index cf0a63f05b..08ae5d0d1f 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -31,19 +31,12 @@ class LoanedCommandInterface public: using Deleter = std::function; - [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedCommandInterface( - CommandInterface & command_interface) + explicit LoanedCommandInterface(CommandInterface::SharedPtr command_interface) : LoanedCommandInterface(command_interface, nullptr) { } - [[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface( - CommandInterface & command_interface, Deleter && deleter) - : command_interface_(command_interface), deleter_(std::forward(deleter)) - { - } - - LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) + explicit LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) : command_interface_(*command_interface), deleter_(std::forward(deleter)) { } @@ -63,7 +56,7 @@ class LoanedCommandInterface get_name().c_str(), get_value_statistics_.timeout_counter, (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, get_value_statistics_.failed_counter, - (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, get_value_statistics_.total_counter); RCLCPP_WARN_EXPRESSION( rclcpp::get_logger(get_name()), @@ -73,7 +66,7 @@ class LoanedCommandInterface get_name().c_str(), set_value_statistics_.timeout_counter, (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, set_value_statistics_.failed_counter, - (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter, + (set_value_statistics_.failed_counter * 100.0) / set_value_statistics_.total_counter, set_value_statistics_.total_counter); if (deleter_) { @@ -85,13 +78,6 @@ class LoanedCommandInterface const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } - [[deprecated( - "Replaced by get_name method, which is semantically more correct")]] const std::string - get_full_name() const - { - return command_interface_.get_name(); - } - const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } /** @@ -128,22 +114,6 @@ class LoanedCommandInterface return true; } - [[deprecated( - "Use std::optional get_optional() instead to retrieve the value. This method will be " - "removed by the ROS 2 Kilted Kaiju release.")]] - double get_value() const - { - double value = std::numeric_limits::quiet_NaN(); - if (get_value(value)) - { - return value; - } - else - { - return std::numeric_limits::quiet_NaN(); - } - } - /** * @brief Get the value of the command interface. * @tparam T The type of the value to be retrieved. @@ -180,41 +150,16 @@ class LoanedCommandInterface } /** - * @brief Get the value of the command interface. - * @tparam T The type of the value to be retrieved. - * @param value The value of the command interface. - * @param max_tries The maximum number of tries to get the value. - * @return true if the value is accessed successfully, false otherwise. - * - * @note The method is thread-safe and non-blocking. - * @note When different threads access the internal handle at same instance, and if they are - * unable to lock the handle to access the value, the handle returns false. If the operation is - * successful, the value is updated and returns true. - * @note The method will try to get the value max_tries times before returning false. The method - * will yield the thread between tries. If the value is updated successfully, the method returns - * true immediately. + * @brief Get the data type of the command interface. + * @return The data type of the command interface. */ - template - [[deprecated( - "Use std::optional get_optional() instead to retrieve the value. This method will be " - "removed by the ROS 2 Kilted Kaiju release.")]] [[nodiscard]] bool - get_value(T & value, unsigned int max_tries = 10) const - { - unsigned int nr_tries = 0; - ++get_value_statistics_.total_counter; - while (!command_interface_.get_value(value)) - { - ++get_value_statistics_.failed_counter; - ++nr_tries; - if (nr_tries == max_tries) - { - ++get_value_statistics_.timeout_counter; - return false; - } - std::this_thread::yield(); - } - return true; - } + HandleDataType get_data_type() const { return command_interface_.get_data_type(); } + + /** + * @brief Check if the state interface can be casted to double. + * @return True if the state interface can be casted to double, false otherwise. + */ + bool is_castable_to_double() const { return command_interface_.is_castable_to_double(); } protected: CommandInterface & command_interface_; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index f822a3f577..ab2720af37 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -30,24 +30,12 @@ class LoanedStateInterface public: using Deleter = std::function; - [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedStateInterface( - const StateInterface & state_interface) - : LoanedStateInterface(state_interface, nullptr) - { - } - - [[deprecated("Replaced by the new version using shared_ptr")]] LoanedStateInterface( - const StateInterface & state_interface, Deleter && deleter) - : state_interface_(state_interface), deleter_(std::forward(deleter)) - { - } - explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter) + explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter) : state_interface_(*state_interface), deleter_(std::forward(deleter)) { } @@ -67,7 +55,7 @@ class LoanedStateInterface state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter, (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, get_value_statistics_.failed_counter, - (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, get_value_statistics_.total_counter); if (deleter_) { @@ -79,31 +67,8 @@ class LoanedStateInterface const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } - [[deprecated( - "Replaced by get_name method, which is semantically more correct")]] const std::string - get_full_name() const - { - return state_interface_.get_name(); - } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - [[deprecated( - "Use std::optional get_optional() instead to retrieve the value. This method will be " - "removed by the ROS 2 Kilted Kaiju release.")]] - double get_value() const - { - double value = std::numeric_limits::quiet_NaN(); - if (get_value(value)) - { - return value; - } - else - { - return std::numeric_limits::quiet_NaN(); - } - } - /** * @brief Get the value of the state interface. * @tparam T The type of the value to be retrieved. @@ -140,41 +105,16 @@ class LoanedStateInterface } /** - * @brief Get the value of the state interface. - * @tparam T The type of the value to be retrieved. - * @param value The value of the state interface. - * @param max_tries The maximum number of tries to get the value. - * @return true if the value is accessed successfully, false otherwise. - * - * @note The method is thread-safe and non-blocking. - * @note When different threads access the same handle at same instance, and if they are unable to - * lock the handle to access the value, the handle returns false. If the operation is successful, - * the value is updated and returns true. - * @note The method will try to get the value max_tries times before returning false. The method - * will yield the thread between tries. If the value is retrieved successfully, the method updates - * the value and returns true immediately. + * @brief Get the data type of the state interface. + * @return The data type of the state interface. */ - template - [[deprecated( - "Use std::optional get_optional() instead to retrieve the value. This method will be " - "removed by the ROS 2 Kilted Kaiju release.")]] [[nodiscard]] bool - get_value(T & value, unsigned int max_tries = 10) const - { - unsigned int nr_tries = 0; - ++get_value_statistics_.total_counter; - while (!state_interface_.get_value(value)) - { - ++get_value_statistics_.failed_counter; - ++nr_tries; - if (nr_tries == max_tries) - { - ++get_value_statistics_.timeout_counter; - return false; - } - std::this_thread::yield(); - } - return true; - } + HandleDataType get_data_type() const { return state_interface_.get_data_type(); } + + /** + * @brief Check if the state interface can be casted to double. + * @return True if the state interface can be casted to double, false otherwise. + */ + bool is_castable_to_double() const { return state_interface_.is_castable_to_double(); } protected: const StateInterface & state_interface_; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 34bfb5affc..eb718c30a1 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -23,13 +23,18 @@ #include "hardware_interface/actuator.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#pragma GCC diagnostic pop #include "hardware_interface/sensor.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/resource_manager_params.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" @@ -40,7 +45,7 @@ class ControllerManager; struct HardwareReadWriteStatus { - bool ok; + return_type result; std::vector failed_hardware_names; }; @@ -95,6 +100,19 @@ class ResourceManager const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, bool activate_all = false, const unsigned int update_rate = 100); + /// Constructor for the Resource Manager. + /** + * The implementation uses the ResourceManagerParams to load the specified urdf and initializes + * the hardware components listed within as well as populate their respective state and command + * interfaces. + * + * \param[in] params ResourceManagerParams containing the parameters for the ResourceManager. + * \param[in] load boolean argument indicating if the components should be loaded and + * initialized. If false, the ResourceManager will not load any components and will only + * initialize the ResourceManager with the given parameters. + */ + explicit ResourceManager(const hardware_interface::ResourceManagerParams & params, bool load); + ResourceManager(const ResourceManager &) = delete; virtual ~ResourceManager(); @@ -117,7 +135,7 @@ class ResourceManager * \returns false if URDF validation has failed. */ virtual bool load_and_initialize_components( - const std::string & urdf, const unsigned int update_rate = 100); + const hardware_interface::ResourceManagerParams & params); /** * @brief Import joint limiters from the URDF. @@ -166,6 +184,14 @@ class ResourceManager */ bool state_interface_is_available(const std::string & name) const; + /// Gets the data type of the state interface. + /** + * \param[in] name string identifying the interface to check. + * \return data type of the state interface. + * \throws std::runtime_error if the state interface does not exist. + */ + std::string get_state_interface_data_type(const std::string & name) const; + /// Add controllers' exported state interfaces to resource manager. /** * Interface for transferring management of exported state interfaces to resource manager. @@ -336,6 +362,14 @@ class ResourceManager */ bool command_interface_is_available(const std::string & interface) const; + /// Gets the data type of the command interface. + /** + * \param[in] name string identifying the interface to check. + * \return data type of the command interface. + * \throws std::runtime_error if the command interface does not exist. + */ + std::string get_command_interface_data_type(const std::string & name) const; + /// Return the number size_t of loaded actuator components. /** * \return number of actuator components. @@ -364,11 +398,11 @@ class ResourceManager * not be called when a controller is running. * \note given that no hardware_info is available, the component has to be configured * externally and prior to the call to import. - * \param[in] actuator pointer to the actuator interface. + * \param[in] system pointer to the system interface. * \param[in] hardware_info hardware info */ void import_component( - std::unique_ptr actuator, const HardwareInfo & hardware_info); + std::unique_ptr system, const HardwareComponentParams & params); /// Import a hardware component which is not listed in the URDF /** @@ -380,11 +414,12 @@ class ResourceManager * not be called when a controller is running. * \note given that no hardware_info is available, the component has to be configured * externally and prior to the call to import. - * \param[in] sensor pointer to the sensor interface. - * \param[in] hardware_info hardware info + * \param[in] actuator pointer to the actuator interface. + * \param[in] params Struct of type HardwareComponentParams containing the hardware info + * and other parameters for the component. */ void import_component( - std::unique_ptr sensor, const HardwareInfo & hardware_info); + std::unique_ptr actuator, const HardwareComponentParams & params); /// Import a hardware component which is not listed in the URDF /** @@ -396,11 +431,12 @@ class ResourceManager * not be called when a controller is running. * \note given that no hardware_info is available, the component has to be configured * externally and prior to the call to import. - * \param[in] system pointer to the system interface. - * \param[in] hardware_info hardware info + * \param[in] sensor pointer to the sensor interface. + * \param[in] params Struct of type HardwareComponentParams containing the hardware info + * and other parameters for the component. */ void import_component( - std::unique_ptr system, const HardwareInfo & hardware_info); + std::unique_ptr sensor, const HardwareComponentParams & params); /// Return status for all components. /** @@ -408,6 +444,19 @@ class ResourceManager */ const std::unordered_map & get_components_status(); + /// Return the unordered map of hard joint limits. + /** + * \return unordered map of hard joint limits. + */ + const std::unordered_map & get_hard_joint_limits() const; + + /// Return the unordered map of soft joint limits. + /** + * \return unordered map of soft joint limits. + */ + const std::unordered_map & get_soft_joint_limits() + const; + /// Prepare the hardware components for a new command interface mode /** * Hardware components are asked to prepare a new command interface claim. @@ -523,6 +572,8 @@ class ResourceManager rclcpp::Clock::SharedPtr get_clock() const; bool components_are_loaded_and_initialized_ = false; + bool allow_controller_activation_with_inactive_hardware_ = false; + bool return_failed_hardware_names_on_return_deactivate_write_cycle_ = true; mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; @@ -534,6 +585,13 @@ class ResourceManager void release_command_interface(const std::string & key); + // Note this was added in #2323 and is a temporary addition to be backwards compatible with the + // original constructors. This is planned to be removed in a future PR along with the + // aforementioned constructors. + hardware_interface::ResourceManagerParams constructParams( + rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, const std::string & urdf = std::string(), + bool activate_all = false, unsigned int update_rate = 100); + std::unordered_map claimed_command_interface_map_; std::unique_ptr resource_storage_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index f3e2bce9cf..e3e7310bd5 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -15,81 +15,11 @@ #ifndef HARDWARE_INTERFACE__SENSOR_HPP_ #define HARDWARE_INTERFACE__SENSOR_HPP_ -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/hardware_component.hpp" #include "hardware_interface/sensor_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/statistics_types.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { -class SensorInterface; - -class Sensor final -{ -public: - Sensor() = default; - - explicit Sensor(std::unique_ptr impl); - - explicit Sensor(Sensor && other) noexcept; - - ~Sensor() = default; - - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); - - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - - const rclcpp_lifecycle::State & configure(); - - const rclcpp_lifecycle::State & cleanup(); - - const rclcpp_lifecycle::State & shutdown(); - - const rclcpp_lifecycle::State & activate(); - - const rclcpp_lifecycle::State & deactivate(); - - const rclcpp_lifecycle::State & error(); - - std::vector export_state_interfaces(); - - const std::string & get_name() const; - - const std::string & get_group_name() const; - - const rclcpp_lifecycle::State & get_lifecycle_state() const; - - const rclcpp::Time & get_last_read_time() const; - - const HardwareComponentStatisticsCollector & get_read_statistics() const; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); - - return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } - - 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_; - // Component statistics - HardwareComponentStatisticsCollector read_statistics_; -}; - +using Sensor = HardwareComponent; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__SENSOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index fd9f8bf98a..dffb700114 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,33 +15,12 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ -#include -#include -#include -#include -#include #include -#include "hardware_interface/component_parser.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/introspection.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#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" +#include "hardware_interface/hardware_component_interface.hpp" namespace hardware_interface { - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /// Virtual Class to implement when integrating a stand-alone sensor into ros2_control. /** * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). @@ -72,353 +51,15 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * ACTIVE (on_activate): * States can be read. */ -class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class SensorInterface : public HardwareComponentInterface { public: - SensorInterface() - : lifecycle_state_( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), - sensor_logger_(rclcpp::get_logger("sensor_interface")) - { - } - - /// SensorInterface copy constructor is actively deleted. - /** - * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid - * failed or simultaneous access to hardware. - */ - SensorInterface(const SensorInterface & other) = delete; - - SensorInterface(SensorInterface && other) = delete; - - virtual ~SensorInterface() = default; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] logger Logger for the hardware component. - * \param[in] clock_interface pointer to the clock interface. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]] - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) - { - return this->init(hardware_info, logger, clock_interface->get_clock()); - } - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] clock pointer to the resource manager clock. - * \param[in] logger Logger for the hardware component. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - { - sensor_clock_ = clock; - 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); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] hardware_info structure with data from URDF. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - 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; - }; - - /// Exports all state interfaces for this hardware interface. - /** - * Old way of exporting the StateInterfaces. If a empty vector is returned then - * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned - * then the exporting of the StateInterfaces is only done with this function and the ownership is - * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not - * be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " - "Exporting is handled " - "by the Framework.")]] virtual std::vector - export_state_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_state_interfaces() and only when empty vector is returned call - // on_export_state_interfaces() - return {}; - } - - /** - * Override this method to export custom StateInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_state_interfaces_ map. - * - * \return vector of descriptions to the unlisted StateInterfaces - */ - virtual std::vector - export_unlisted_state_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the sensor_interface. - * - * \return vector of shared pointers to the created and stored StateInterfaces - */ - virtual std::vector on_export_state_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_state_interface_descriptions(); - - std::vector state_interfaces; - state_interfaces.reserve( - unlisted_interface_descriptions.size() + sensor_state_interfaces_.size() + - joint_state_interfaces_.size()); - - // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_state_interfaces_.insert(std::make_pair(name, description)); - auto state_interface = std::make_shared(description); - sensor_states_map_.insert(std::make_pair(name, state_interface)); - unlisted_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : sensor_state_interfaces_) - { - // TODO(Manuel) check for duplicates otherwise only the first appearance of "name" is inserted - auto state_interface = std::make_shared(descr); - sensor_states_map_.insert(std::make_pair(name, state_interface)); - sensor_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : joint_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - sensor_states_map_.insert(std::make_pair(name, state_interface)); - joint_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } + std::vector on_export_command_interfaces() override { return {}; } - 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. - */ - HardwareComponentCycleStatus trigger_read( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - const auto result = read_async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - status.result = result.second; - const auto execution_time = read_async_handler_->get_last_execution_time(); - if (execution_time.count() > 0) - { - status.execution_time = execution_time; - } - if (!status.successful) - { - 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()); - status.result = return_type::OK; - return status; - } - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = read(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Read the current state values from the actuator. - /** - * 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. - */ - virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - const std::string & get_name() const { return info_.name; } - - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ - const std::string & get_group_name() const { return info_.group; } - - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ - const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } - - /// Set life-cycle state of the actuator hardware. - /** - * \return state. - */ - void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) - { - lifecycle_state_ = new_state; - } - - template - void set_state(const std::string & interface_name, const T & value) - { - auto it = sensor_states_map_.find(interface_name); - if (it == sensor_states_map_.end()) - { - throw std::runtime_error( - "State interface not found: " + interface_name + - " in sensor hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_state(const std::string & interface_name) const + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) final { - auto it = sensor_states_map_.find(interface_name); - if (it == sensor_states_map_.end()) - { - throw std::runtime_error( - "State interface not found: " + interface_name + - " in sensor hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - "Failed to get state value from interface: " + interface_name + - ". This should not happen."); - } - return opt_value.value(); + return return_type::OK; } - - /// Get the logger of the SensorInterface. - /** - * \return logger of the SensorInterface. - */ - rclcpp::Logger get_logger() const { return sensor_logger_; } - - /// Get the clock of the SensorInterface. - /** - * \return clock of the SensorInterface. - */ - rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; } - - /// Get the hardware info of the SensorInterface. - /** - * \return hardware info of the SensorInterface. - */ - const HardwareInfo & get_hardware_info() const { return info_; } - - /// Enable or disable introspection of the sensor hardware. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ - void enable_introspection(bool enable) - { - if (enable) - { - stats_registrations_.enableAll(); - } - else - { - stats_registrations_.disableAll(); - } - } - -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::Clock::SharedPtr sensor_clock_; - rclcpp::Logger sensor_logger_; - // interface names to Handle accessed through getters/setters - std::unordered_map sensor_states_map_; - -protected: - pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index c5c452681e..4543020cfd 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -15,98 +15,11 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_HPP_ #define HARDWARE_INTERFACE__SYSTEM_HPP_ -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/hardware_component.hpp" #include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/statistics_types.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { -class SystemInterface; - -class System final -{ -public: - System() = default; - - explicit System(std::unique_ptr impl); - - explicit System(System && other) noexcept; - - ~System() = default; - - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); - - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - - const rclcpp_lifecycle::State & configure(); - - const rclcpp_lifecycle::State & cleanup(); - - const rclcpp_lifecycle::State & shutdown(); - - const rclcpp_lifecycle::State & activate(); - - const rclcpp_lifecycle::State & deactivate(); - - const rclcpp_lifecycle::State & error(); - - std::vector export_state_interfaces(); - - std::vector export_command_interfaces(); - - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - const std::string & get_name() const; - - const std::string & get_group_name() const; - - const rclcpp_lifecycle::State & get_lifecycle_state() const; - - const rclcpp::Time & get_last_read_time() const; - - const rclcpp::Time & get_last_write_time() const; - - const HardwareComponentStatisticsCollector & get_read_statistics() const; - - const HardwareComponentStatisticsCollector & get_write_statistics() const; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); - - return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); - - 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_; - // Component statistics - HardwareComponentStatisticsCollector read_statistics_; - HardwareComponentStatisticsCollector write_statistics_; -}; - +using System = HardwareComponent; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__SYSTEM_HPP_ diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 8e5d7959fb..e7bc4cffa3 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,36 +15,10 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/component_parser.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/introspection.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "hardware_interface/types/trigger_type.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#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" +#include "hardware_interface/hardware_component_interface.hpp" namespace hardware_interface { - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** * @brief Virtual Class to implement when integrating a complex system into ros2_control. * @@ -68,10 +42,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. - * States can be read and command interfaces are available. - * - * As of now, it is left to the hardware component implementation to continue using the command - *received from the ``CommandInterfaces`` or to skip them completely. + * States can be read, but command interfaces are not available. * * FINALIZED (on_shutdown): * Hardware interface is ready for unloading/destruction. @@ -79,7 +50,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * * ACTIVE (on_activate): * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. - * Command interfaces available. + * Command interfaces are available. * * \todo * Implement @@ -92,622 +63,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface *HW_IF_EFFORT. */ -class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class SystemInterface : public HardwareComponentInterface { public: - SystemInterface() - : lifecycle_state_( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), - system_logger_(rclcpp::get_logger("system_interface")) - { - } - - /// SystemInterface copy constructor is actively deleted. - /** - * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid - * failed or simultaneous access to hardware. - */ - SystemInterface(const SystemInterface & other) = delete; - - SystemInterface(SystemInterface && other) = delete; - - virtual ~SystemInterface() = default; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] logger Logger for the hardware component. - * \param[in] clock_interface pointer to the clock interface. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]] - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) - { - return this->init(hardware_info, logger, clock_interface->get_clock()); - } - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] clock pointer to the resource manager clock. - * \param[in] logger Logger for the hardware component. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - { - system_clock_ = clock; - 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) - { - const auto read_start_time = std::chrono::steady_clock::now(); - const auto ret_read = read(time, period); - const auto read_end_time = std::chrono::steady_clock::now(); - read_return_info_.store(ret_read, std::memory_order_release); - read_execution_time_.store( - std::chrono::duration_cast(read_end_time - read_start_time), - std::memory_order_release); - if (ret_read != return_type::OK) - { - return ret_read; - } - const auto write_start_time = std::chrono::steady_clock::now(); - const auto ret_write = write(time, period); - const auto write_end_time = std::chrono::steady_clock::now(); - write_return_info_.store(ret_write, std::memory_order_release); - write_execution_time_.store( - std::chrono::duration_cast(write_end_time - write_start_time), - std::memory_order_release); - return ret_write; - }, - info_.thread_priority); - async_handler_->start_thread(); - } - return on_init(hardware_info); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] hardware_info structure with data from URDF. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - 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_); - parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); - parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); - parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); - return CallbackReturn::SUCCESS; - }; - - /// Exports all state interfaces for this hardware interface. - /** - * Old way of exporting the StateInterfaces. If a empty vector is returned then - * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned - * then the exporting of the StateInterfaces is only done with this function and the ownership is - * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not - * be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " - "Exporting is handled " - "by the Framework.")]] virtual std::vector - export_state_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_state_interfaces() and only when empty vector is returned call - // on_export_state_interfaces() - return {}; - } - - /** - * Override this method to export custom StateInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_state_interfaces_ map. - * - * \return vector of descriptions to the unlisted StateInterfaces - */ - virtual std::vector - export_unlisted_state_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * \return vector of shared pointers to the created and stored StateInterfaces - */ - virtual std::vector on_export_state_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_state_interface_descriptions(); - - std::vector state_interfaces; - state_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + - sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); - - // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_state_interfaces_.insert(std::make_pair(name, description)); - auto state_interface = std::make_shared(description); - system_states_.insert(std::make_pair(name, state_interface)); - unlisted_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : joint_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - system_states_.insert(std::make_pair(name, state_interface)); - joint_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - for (const auto & [name, descr] : sensor_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - system_states_.insert(std::make_pair(name, state_interface)); - sensor_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - for (const auto & [name, descr] : gpio_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - system_states_.insert(std::make_pair(name, state_interface)); - gpio_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - return state_interfaces; - } - - /// Exports all command interfaces for this hardware interface. - /** - * Old way of exporting the CommandInterfaces. If a empty vector is returned then - * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is - * returned then the exporting of the CommandInterfaces is only done with this function and the - * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., - * can then not be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_command_interfaces() method. " - "Exporting is " - "handled " - "by the Framework.")]] virtual std::vector - export_command_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_command_interfaces() and only when empty vector is returned call - // on_export_command_interfaces() - return {}; - } - - /** - * Override this method to export custom CommandInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_command_interfaces_ map. - * - * \return vector of descriptions to the unlisted CommandInterfaces - */ - virtual std::vector - export_unlisted_command_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * \return vector of shared pointers to the created and stored CommandInterfaces - */ - virtual std::vector on_export_command_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_command_interface_descriptions(); - - std::vector command_interfaces; - command_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + - gpio_command_interfaces_.size()); - - // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_command_interfaces_.insert(std::make_pair(name, description)); - auto command_interface = std::make_shared(description); - system_commands_.insert(std::make_pair(name, command_interface)); - unlisted_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - for (const auto & [name, descr] : joint_command_interfaces_) - { - auto command_interface = std::make_shared(descr); - system_commands_.insert(std::make_pair(name, command_interface)); - joint_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - for (const auto & [name, descr] : gpio_command_interfaces_) - { - auto command_interface = std::make_shared(descr); - system_commands_.insert(std::make_pair(name, command_interface)); - gpio_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - return command_interfaces; - } - - /// Prepare for a new command interface switch. - /** - * Prepare for any mode-switching required by the new command interface combination. - * - * \note This is a non-realtime evaluation of whether a set of command interface claims are - * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be prepared, or - * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. - */ - virtual return_type prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - return return_type::OK; - } - - // Perform switching to the new command interface. - /** - * Perform the mode-switching for the new command interface combination. - * - * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \param[in] start_interfaces vector of string identifiers for the command interfaces - * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces - * stopping. \return return_type::OK if the new command interface combination can be switched to, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. - */ - virtual return_type perform_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - 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. - */ - HardwareComponentCycleStatus trigger_read( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.result = read_return_info_.load(std::memory_order_acquire); - const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); - if (read_exec_time.count() > 0) - { - status.execution_time = read_exec_time; - } - const auto result = async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - if (!status.successful) - { - RCLCPP_WARN( - get_logger(), - "Trigger read/write called while the previous async trigger is still in progress for " - "hardware interface : '%s'. Failed to trigger read/write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = read(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Read the current state values from the actuator. - /** - * 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. - */ - 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. - */ - HardwareComponentCycleStatus trigger_write( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.successful = true; - const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); - if (write_exec_time.count() > 0) - { - status.execution_time = write_exec_time; - } - status.result = write_return_info_.load(std::memory_order_acquire); - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = write(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Write the current command values to the actuator. - /** - * The physical hardware shall be updated with the latest value from - * the exported command interfaces. - * - * \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. - */ - virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - const std::string & get_name() const { return info_.name; } - - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ - const std::string & get_group_name() const { return info_.group; } - - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ - const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } - - /// Set life-cycle state of the actuator hardware. - /** - * \return state. - */ - void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) - { - lifecycle_state_ = new_state; - } - - template - void set_state(const std::string & interface_name, const T & value) - { - auto it = system_states_.find(interface_name); - if (it == system_states_.end()) - { - throw std::runtime_error( - "State interface not found: " + interface_name + - " in system hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_state(const std::string & interface_name) const - { - auto it = system_states_.find(interface_name); - if (it == system_states_.end()) - { - throw std::runtime_error( - "State interface not found: " + interface_name + - " in system hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - "Failed to get state value from interface: " + interface_name + - ". This should not happen."); - } - return opt_value.value(); - } - - void set_command(const std::string & interface_name, const double & value) - { - auto it = system_commands_.find(interface_name); - if (it == system_commands_.end()) - { - throw std::runtime_error( - "Command interface not found: " + interface_name + - " in system hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_command(const std::string & interface_name) const - { - auto it = system_commands_.find(interface_name); - if (it == system_commands_.end()) - { - throw std::runtime_error( - "Command interface not found: " + interface_name + - " in system hardware component: " + info_.name + ". This should not happen."); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - "Failed to get command value from interface: " + interface_name + - ". This should not happen."); - } - return opt_value.value(); - } - - /// Get the logger of the SystemInterface. - /** - * \return logger of the SystemInterface. - */ - rclcpp::Logger get_logger() const { return system_logger_; } - - /// Get the clock of the SystemInterface. - /** - * \return clock of the SystemInterface. - */ - rclcpp::Clock::SharedPtr get_clock() const { return system_clock_; } - - /// Get the hardware info of the SystemInterface. - /** - * \return hardware info of the SystemInterface. - */ - const HardwareInfo & get_hardware_info() const { return info_; } - - /// Prepare for the activation of the hardware. - /** - * This method is called before the hardware is activated by the resource manager. - */ - void prepare_for_activation() - { - read_return_info_.store(return_type::OK, std::memory_order_release); - read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - write_return_info_.store(return_type::OK, std::memory_order_release); - write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - } - - /// Enable or disable introspection of the hardware. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ - void enable_introspection(bool enable) - { - if (enable) - { - stats_registrations_.enableAll(); - } - else - { - stats_registrations_.disableAll(); - } - } - -protected: - HardwareInfo info_; - // interface names to InterfaceDescription - std::unordered_map joint_state_interfaces_; - std::unordered_map joint_command_interfaces_; - - std::unordered_map sensor_state_interfaces_; - - std::unordered_map gpio_state_interfaces_; - std::unordered_map gpio_command_interfaces_; - - std::unordered_map unlisted_state_interfaces_; - std::unordered_map unlisted_command_interfaces_; - - rclcpp_lifecycle::State lifecycle_state_; - std::unique_ptr> async_handler_; - - // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector joint_states_; - std::vector joint_commands_; - - std::vector sensor_states_; - - std::vector gpio_states_; - std::vector gpio_commands_; - - std::vector unlisted_states_; - std::vector unlisted_commands_; - -private: - rclcpp::Clock::SharedPtr system_clock_; - rclcpp::Logger system_logger_; - // interface names to Handle accessed through getters/setters - std::unordered_map system_states_; - std::unordered_map system_commands_; - std::atomic read_return_info_ = return_type::OK; - std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); - std::atomic write_return_info_ = return_type::OK; - std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); - -protected: - pal_statistics::RegistrationsRAII stats_registrations_; + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_component_interface_params.hpp b/hardware_interface/include/hardware_interface/types/hardware_component_interface_params.hpp new file mode 100644 index 0000000000..8cc1ee9d33 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_component_interface_params.hpp @@ -0,0 +1,51 @@ +// Copyright 2025 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__HARDWARE_COMPONENT_INTERFACE_PARAMS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_INTERFACE_PARAMS_HPP_ + +#include +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_component_params.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace hardware_interface +{ + +/** + * @brief Parameters required for the initialization of a specific hardware component plugin. + * Typically used for on_init methods of hardware interfaces, and is parsed by the user. + * This struct is typically populated with data from HardwareComponentParams from each component. + */ +struct HardwareComponentInterfaceParams +{ + HardwareComponentInterfaceParams() = default; + /** + * @brief Reference to the HardwareInfo struct for this specific component, + * parsed from the URDF. The HardwareInfo object's lifetime must be guaranteed + * by the caller (e.g., ResourceManager) for the duration this struct is used. + */ + hardware_interface::HardwareInfo hardware_info; + + /** + * @brief Weak pointer to the rclcpp::Executor instance. Hardware components + * can use this (after locking) to add their own internal ROS 2 nodes + * to the ControllerManager's executor. + */ + rclcpp::Executor::WeakPtr executor; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_INTERFACE_PARAMS_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_component_params.hpp b/hardware_interface/include/hardware_interface/types/hardware_component_params.hpp new file mode 100644 index 0000000000..4327349d91 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_component_params.hpp @@ -0,0 +1,69 @@ +// Copyright 2025 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__HARDWARE_COMPONENT_PARAMS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_PARAMS_HPP_ + +#include +#include +#include "hardware_interface/hardware_info.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace hardware_interface +{ + +/** + * @brief Parameters required for the initialization of a specific hardware component plugin. + * Typically used for init, initialise and load methods of hardware components. + * This struct is typically not accessible to user. + * This struct is typically populated with data from ResourceManagerParams. + */ +struct HardwareComponentParams +{ + HardwareComponentParams() = default; + /** + * @brief Reference to the HardwareInfo struct for this specific component, + * parsed from the URDF. The HardwareInfo object's lifetime must be guaranteed + * by the caller (e.g., ResourceManager) for the duration this struct is used. + */ + hardware_interface::HardwareInfo hardware_info; + + /** + * @brief A logger instance taken from resource manager + */ + rclcpp::Logger logger = rclcpp::get_logger("resource_manager"); + + /** + * @brief Shared pointer to the rclcpp::Clock to be used by this hardware component. + * Typically, this is the same clock used by the ResourceManager/ControllerManager. + */ + rclcpp::Clock::SharedPtr clock = nullptr; + + /** + * @brief The namespace used by the hardware component's internal node. + * This is typically same as the controller manager's node namespace. + */ + std::string node_namespace = ""; + + /** + * @brief Weak pointer to the rclcpp::Executor instance. Hardware components + * can use this (after locking) to add their own internal ROS 2 nodes + * to the ControllerManager's executor. + */ + rclcpp::Executor::WeakPtr executor; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_PARAMS_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp b/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp new file mode 100644 index 0000000000..5abb695c10 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/resource_manager_params.hpp @@ -0,0 +1,103 @@ +// Copyright 2025 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__RESOURCE_MANAGER_PARAMS_HPP_ +#define HARDWARE_INTERFACE__TYPES__RESOURCE_MANAGER_PARAMS_HPP_ + +#include +#include +#include "rclcpp/rclcpp.hpp" + +namespace hardware_interface +{ + +/** + * @brief Parameters required for the construction and initial setup of a ResourceManager. + * This struct is typically populated by the ControllerManager. + */ +struct ResourceManagerParams +{ + ResourceManagerParams() = default; + /** + * @brief The URDF string describing the robot's hardware components. + * Can be empty if ResourceManager is constructed without an initial URDF + * and components are loaded later or via other means. + */ + std::string robot_description = ""; + + /** + * @brief Shared pointer to the Clock used by the ResourceManager. + * This is typically obtained from the node via get_clock(). + */ + rclcpp::Clock::SharedPtr clock = nullptr; + + /** + * @brief Logger instance used by the ResourceManager. + * This is typically obtained from the node via get_logger(). + */ + rclcpp::Logger logger = rclcpp::get_logger("resource_manager"); + + /** + * @brief The namespace used by the ResourceManager and its components. + * This is typically same as the controller manager's node namespace. + */ + std::string node_namespace = ""; + + /** + * @brief Shared pointer to the rclcpp::Executor instance that the + * ResourceManager and its components (including plugins that opt-in) will use. + * This is typically the ControllerManager's main executor. + */ + rclcpp::Executor::SharedPtr executor = nullptr; + + /** + * @brief Flag indicating if all hardware components found in the URDF + * should be automatically activated after successful loading and initialization. + */ + bool activate_all = false; + + /** + * @brief If true, controllers are allowed to claim resources from inactive hardware components. + * If false, controllers can only claim resources from active hardware components. + * Moreover, when the hardware component returns DEACTIVATE on read/write cycle: If set to true, + * the controllers using those interfaces will continue to run. If set to false, the controllers + * using those interfaces will be deactivated. + * @warning Allowing control with inactive hardware is not recommended for safety reasons. + * Use with caution only if you really know what you are doing. + * @note This parameter might be deprecated or removed in the future releases. Please use with + * caution. + */ + bool allow_controller_activation_with_inactive_hardware = false; + + /** + * @brief If true, when a hardware component returns DEACTIVATE on the write cycle, + * its name will be included in the returned HardwareReadWriteStatus.failed_hardware_names list. + * If false, the names of such hardware components will not be included in that list. + * This can be useful when controllers are allowed to operate with inactive hardware components. + * @note This parameter might be deprecated or removed in future releases. Please use with + * caution. + */ + bool return_failed_hardware_names_on_return_deactivate_write_cycle_ = true; + + /** + * @brief The update rate (in Hz) of the ControllerManager. + * This can be used by ResourceManager to configure asynchronous hardware components + * or for other timing considerations. + */ + unsigned int update_rate = 100; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__RESOURCE_MANAGER_PARAMS_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp index ba22a47c99..78601ffa9c 100644 --- a/hardware_interface/include/hardware_interface/types/statistics_types.hpp +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -17,23 +17,204 @@ #ifndef HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ #define HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ +#include #include #include +#include "hardware_interface/introspection.hpp" #include "libstatistics_collector/moving_average_statistics/moving_average.hpp" #include "libstatistics_collector/moving_average_statistics/types.hpp" +#if !defined(_WIN32) && !defined(__APPLE__) #include "realtime_tools/mutex.hpp" +#define DEFAULT_MUTEX realtime_tools::prio_inherit_mutex +#else +#define DEFAULT_MUTEX std::mutex +#endif namespace ros2_control { +/** + * A class for calculating moving average statistics. This operates in constant memory and constant + * time. Note: reset() must be called manually in order to start a new measurement window. + * + * The statistics calculated are average, maximum, minimum, and standard deviation (population). + * All are calculated online without storing the observation data. Specifically, the average is a + * running sum and the variance is obtained by Welford's online algorithm (reference: + * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford%27s_online_algorithm) + * for standard deviation. + * + * When statistics are not available, e.g. no observations have been made, NaNs are returned. + */ +class MovingAverageStatistics +{ +public: + using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; + MovingAverageStatistics() = default; + + ~MovingAverageStatistics() = default; + + /** + * Returns the arithmetic mean of all data recorded. If no observations have been made, returns + * NaN. + * + * @return The arithmetic mean of all data recorded, or NaN if the sample count is 0. + */ + double get_average() const + { + std::lock_guard lock(mutex_); + return statistics_data_.average; + } + + /** + * Returns the maximum value recorded. If size of list is zero, returns NaN. + * + * @return The maximum value recorded, or NaN if size of data is zero. + */ + double get_max() const + { + std::lock_guard lock(mutex_); + return statistics_data_.max; + } + + /** + * Returns the minimum value recorded. If size of list is zero, returns NaN. + * + * @return The minimum value recorded, or NaN if size of data is zero. + */ + double get_min() const + { + std::lock_guard lock(mutex_); + return statistics_data_.min; + } + + /** + * Returns the standard deviation (population) of all data recorded. If size of list is zero, + * returns NaN. + * + * Variance is obtained by Welford's online algorithm, + * see + * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford%27s_online_algorithm + * + * @return The standard deviation (population) of all data recorded, or NaN if size of data is + * zero. + */ + double get_standard_deviation() const + { + std::lock_guard lock(mutex_); + return statistics_data_.standard_deviation; + } + + /** + * Return a StatisticData object, containing average, minimum, maximum, standard deviation + * (population), and sample count. For the case of no observations, the average, min, max, and + * standard deviation are NaN. + * + * @return StatisticData object, containing average, minimum, maximum, standard deviation + * (population), and sample count. + */ + const StatisticData & get_statistics_const_ptr() const + { + std::lock_guard lock(mutex_); + return statistics_data_; + } + + StatisticData get_statistics() const + { + std::lock_guard lock(mutex_); + return statistics_data_; + } + + /** + * Get the current measurement value. + * This is the last value added to the statistics collector. + * + * @return The current measurement value, or NaN if no measurements have been made. + */ + const double & get_current_measurement_const_ptr() const + { + std::lock_guard lock(mutex_); + return current_measurement_; + } + + double get_current_measurement() const + { + std::lock_guard lock(mutex_); + return current_measurement_; + } + + /** + * Reset all calculated values. Equivalent to a new window for a moving average. + */ + void reset() + { + std::lock_guard lock(mutex_); + statistics_data_.average = 0.0; + statistics_data_.min = std::numeric_limits::max(); + statistics_data_.max = std::numeric_limits::lowest(); + statistics_data_.standard_deviation = 0.0; + statistics_data_.sample_count = 0; + current_measurement_ = std::numeric_limits::quiet_NaN(); + sum_of_square_diff_from_mean_ = 0; + } + + void reset_current_measurement() + { + std::lock_guard lock(mutex_); + current_measurement_ = 0.0; + } + + /** + * Observe a sample for the given window. The input item is used to calculate statistics. + * Note: any input values of NaN will be discarded and not added as a measurement. + * + * @param item The item that was observed + */ + void add_measurement(const double item) + { + std::lock_guard guard{mutex_}; + + current_measurement_ = item; + if (std::isfinite(item)) + { + statistics_data_.sample_count = statistics_data_.sample_count + 1; + const double previous_average = statistics_data_.average; + statistics_data_.average = + previous_average + (current_measurement_ - previous_average) / + static_cast(statistics_data_.sample_count); + statistics_data_.min = std::min(statistics_data_.min, current_measurement_); + statistics_data_.max = std::max(statistics_data_.max, current_measurement_); + sum_of_square_diff_from_mean_ = + sum_of_square_diff_from_mean_ + (current_measurement_ - previous_average) * + (current_measurement_ - statistics_data_.average); + statistics_data_.standard_deviation = std::sqrt( + sum_of_square_diff_from_mean_ / static_cast(statistics_data_.sample_count)); + } + } + + /** + * Return the number of samples observed + * + * @return the number of samples observed + */ + uint64_t get_count() const + { + std::lock_guard lock(mutex_); + return statistics_data_.sample_count; + } + +private: + mutable DEFAULT_MUTEX mutex_; + StatisticData statistics_data_; + double current_measurement_ = std::numeric_limits::quiet_NaN(); + double sum_of_square_diff_from_mean_ = 0.0; +}; + /** * @brief Data structure to store the statistics of a moving average. The data is protected by a * mutex and the data can be updated and retrieved. */ class MovingAverageStatisticsData { - using MovingAverageStatisticsCollector = - libstatistics_collector::moving_average_statistics::MovingAverageStatistics; using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; public: @@ -47,21 +228,22 @@ class MovingAverageStatisticsData * @brief Update the statistics data with the new statistics data. * @param statistics statistics collector to update the current statistics data. */ - void update_statistics(const std::shared_ptr & statistics) + void update_statistics(const std::shared_ptr & statistics) { - std::unique_lock lock(mutex_); - if (statistics->GetCount() > 0) + std::unique_lock lock(mutex_); + if (statistics->get_count() > 0) { - statistics_data.average = statistics->Average(); - statistics_data.min = statistics->Min(); - statistics_data.max = statistics->Max(); - statistics_data.standard_deviation = statistics->StandardDeviation(); - statistics_data.sample_count = statistics->GetCount(); - statistics_data = statistics->GetStatistics(); + statistics_data_.average = statistics->get_average(); + statistics_data_.min = statistics->get_min(); + statistics_data_.max = statistics->get_max(); + statistics_data_.standard_deviation = statistics->get_standard_deviation(); + statistics_data_.sample_count = statistics->get_count(); + statistics_data_ = statistics->get_statistics(); + current_data_ = statistics->get_current_measurement(); } - if (statistics->GetCount() >= reset_statistics_sample_count_) + if (statistics->get_count() >= reset_statistics_sample_count_) { - statistics->Reset(); + statistics->reset(); } } @@ -71,17 +253,17 @@ class MovingAverageStatisticsData */ void set_reset_statistics_sample_count(unsigned int reset_sample_count) { - std::unique_lock lock(mutex_); + std::unique_lock lock(mutex_); reset_statistics_sample_count_ = reset_sample_count; } void reset() { - statistics_data.average = std::numeric_limits::quiet_NaN(); - statistics_data.min = std::numeric_limits::quiet_NaN(); - statistics_data.max = std::numeric_limits::quiet_NaN(); - statistics_data.standard_deviation = std::numeric_limits::quiet_NaN(); - statistics_data.sample_count = 0; + statistics_data_.average = std::numeric_limits::quiet_NaN(); + statistics_data_.min = std::numeric_limits::quiet_NaN(); + statistics_data_.max = std::numeric_limits::quiet_NaN(); + statistics_data_.standard_deviation = std::numeric_limits::quiet_NaN(); + statistics_data_.sample_count = 0; } /** @@ -90,17 +272,25 @@ class MovingAverageStatisticsData */ const StatisticData & get_statistics() const { - std::unique_lock lock(mutex_); - return statistics_data; + std::unique_lock lock(mutex_); + return statistics_data_; + } + + const double & get_current_data() const + { + std::unique_lock lock(mutex_); + return current_data_; } private: + /// Mutex to protect the statistics data + mutable DEFAULT_MUTEX mutex_; /// Statistics data - StatisticData statistics_data; + StatisticData statistics_data_; + /// Current data value, used to calculate the statistics + double current_data_ = std::numeric_limits::quiet_NaN(); /// Number of samples to reset the statistics unsigned int reset_statistics_sample_count_ = std::numeric_limits::max(); - /// Mutex to protect the statistics data - mutable realtime_tools::prio_inherit_mutex mutex_; }; } // namespace ros2_control @@ -114,27 +304,24 @@ struct HardwareComponentStatisticsCollector { HardwareComponentStatisticsCollector() { - execution_time = std::make_shared(); - periodicity = std::make_shared(); + execution_time = std::make_shared(); + periodicity = std::make_shared(); } - using MovingAverageStatisticsCollector = - libstatistics_collector::moving_average_statistics::MovingAverageStatistics; - /** * @brief Resets the internal statistics of the execution time and periodicity statistics * collectors. */ void reset_statistics() { - execution_time->Reset(); - periodicity->Reset(); + execution_time->reset(); + periodicity->reset(); } /// Execution time statistics collector - std::shared_ptr execution_time = nullptr; + std::shared_ptr execution_time = nullptr; /// Periodicity statistics collector - std::shared_ptr periodicity = nullptr; + std::shared_ptr periodicity = nullptr; }; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 418924c007..707214ae0d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -20,7 +20,10 @@ #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/handle.hpp" +#pragma GCC diagnostic pop #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -39,11 +42,17 @@ static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; class GenericSystem : public hardware_interface::SystemInterface { public: - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override; - std::vector export_state_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + std::vector + export_unlisted_command_interface_descriptions() override; return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -71,51 +80,18 @@ class GenericSystem : public hardware_interface::SystemInterface const std::vector standard_interfaces_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_mock_commands_; - std::vector> sensor_states_; - - std::vector gpio_interfaces_; - /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_mock_commands_; - std::vector> gpio_commands_; - std::vector> gpio_states_; + std::vector skip_interfaces_; private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos); - - template bool populate_interfaces( const std::vector & components, - std::vector & interfaces, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces); + std::vector & command_interface_descriptions) const; bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; bool calculate_dynamics_; std::vector joint_control_mode_; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 9121ef95e4..869e0b0992 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.29.0 + 6.0.1 Base classes for hardware abstraction and tooling for them Bence Magyar Denis Štogl @@ -25,6 +25,7 @@ sdformat_urdf tinyxml2_vendor urdf + fmt rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0d70bf0f81..90c68c67d5 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include + #include #include #include @@ -53,6 +55,8 @@ constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; constexpr const auto kLimitsTag = "limits"; +constexpr const auto kPropertiesTag = "properties"; +constexpr const auto kAsyncTag = "async"; constexpr const auto kEnableAttribute = "enable"; constexpr const auto kInitialValueTag = "initial_value"; constexpr const auto kMimicAttribute = "mimic"; @@ -66,6 +70,9 @@ constexpr const auto kOffsetAttribute = "offset"; constexpr const auto kReadWriteRateAttribute = "rw_rate"; constexpr const auto kIsAsyncAttribute = "is_async"; constexpr const auto kThreadPriorityAttribute = "thread_priority"; +constexpr const auto kAffinityCoresAttribute = "affinity"; +constexpr const auto kSchedulingPolicyAttribute = "scheduling_policy"; +constexpr const auto kPrintWarningsAttribute = "print_warnings"; } // namespace @@ -110,7 +117,7 @@ std::string get_attribute_value( if (!attr) { throw std::runtime_error( - "no attribute " + std::string(attribute_name) + " in " + tag_name + " tag"); + fmt::format(FMT_COMPILE("no attribute {} in {} tag"), attribute_name, tag_name)); } return element_it->Attribute(attribute_name); } @@ -198,8 +205,11 @@ std::size_t parse_size_attribute(const tinyxml2::XMLElement * elem) else { throw std::runtime_error( - "Could not parse size tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + s + - "\", but expected a non-zero positive integer."); + fmt::format( + FMT_COMPILE( + "Could not parse size tag in \"{}\". Got \"{}\", but expected a non-zero positive " + "integer."), + elem->Name(), s)); } return size; @@ -246,22 +256,30 @@ unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem) 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."); + fmt::format( + FMT_COMPILE( + "Could not parse rw_rate tag in \"{}\". Got \"{}\", but expected a positive integer."), + elem->Name(), rw_rate)); } 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."); + fmt::format( + FMT_COMPILE( + "Could not parse rw_rate tag in \"{}\". Invalid value: \"{}\", expected a positive " + "integer."), + elem->Name(), attr->Value())); } 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."); + fmt::format( + FMT_COMPILE( + "Could not parse rw_rate tag in \"{}\". Out of range value: \"{}\", expected a positive " + "valid integer."), + elem->Name(), attr->Value())); } } @@ -303,8 +321,11 @@ int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) 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."); + fmt::format( + FMT_COMPILE( + "Could not parse thread_priority tag in \"{}\". Got \"{}\", but expected a non-zero " + "positive integer."), + elem->Name(), s)); } } @@ -394,6 +415,9 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.parameters = parse_parameters_from_xml(params_it); } + interface.data_type = parse_data_type_attribute(interfaces_it); + interface.size = static_cast(parse_size_attribute(interfaces_it)); + return interface; } @@ -487,10 +511,6 @@ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * comp while (command_interfaces_it) { component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); - component.command_interfaces.back().data_type = - parse_data_type_attribute(command_interfaces_it); - component.command_interfaces.back().size = - static_cast(parse_size_attribute(command_interfaces_it)); command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } @@ -499,9 +519,6 @@ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * comp while (state_interfaces_it) { component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it)); - component.state_interfaces.back().data_type = parse_data_type_attribute(state_interfaces_it); - component.state_interfaces.back().size = - static_cast(parse_size_attribute(state_interfaces_it)); state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } @@ -603,9 +620,11 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware) if (found_it == hardware.joints.cend()) { throw std::runtime_error( - "Error while parsing '" + hardware.name + "'. Transmission '" + transmission.name + - "' declared joint '" + joint.name + "' is not available in component '" + hardware.name + - "'."); + fmt::format( + FMT_COMPILE( + "Error while parsing '{}'. Transmission '{}' declared joint '{}' is not available in " + "component '{}'."), + hardware.name, transmission.name, joint.name, hardware.name)); } // copy interface names from their definitions in the component @@ -626,9 +645,11 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware) if (transmission.joints.size() != 1) { throw std::runtime_error( - "Error while parsing '" + hardware.name + - "'. There should be exactly one joint defined in this component but found " + - std::to_string(transmission.joints.size())); + fmt::format( + FMT_COMPILE( + "Error while parsing '{}'. There should be exactly one joint defined in this " + "component but found {}."), + hardware.name, transmission.joints.size())); } transmission.actuators.push_back( @@ -654,8 +675,23 @@ HardwareInfo parse_resource_from_xml( hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it); hardware.is_async = parse_is_async_attribute(ros2_control_it); - hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it) - : std::numeric_limits::max(); + hardware.async_params.thread_priority = hardware.is_async + ? parse_thread_priority_attribute(ros2_control_it) + : std::numeric_limits::max(); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + hardware.thread_priority = hardware.async_params.thread_priority; +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif // Parse everything under ros2_control tag hardware.hardware_plugin_name = ""; @@ -682,6 +718,55 @@ HardwareInfo parse_resource_from_xml( hardware.hardware_parameters = parse_parameters_from_xml(params_it); } } + else if (std::string(kPropertiesTag) == ros2_control_child_it->Name()) + { + const auto * async_it = ros2_control_child_it->FirstChildElement(kAsyncTag); + if (async_it) + { + // Async properties are defined + try + { + if (async_it->FindAttribute(kAffinityCoresAttribute)) + { + hardware.async_params.cpu_affinity_cores = + parse_array(get_attribute_value(async_it, kAffinityCoresAttribute, kAsyncTag)); + } + if (async_it->FindAttribute(kSchedulingPolicyAttribute)) + { + hardware.async_params.scheduling_policy = + to_lower_case(get_attribute_value(async_it, kSchedulingPolicyAttribute, kAsyncTag)); + } + if (async_it->FindAttribute(kThreadPriorityAttribute)) + { + hardware.async_params.thread_priority = parse_thread_priority_attribute(async_it); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + hardware.thread_priority = hardware.async_params.thread_priority; +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + } + if (async_it->FindAttribute(kPrintWarningsAttribute)) + { + hardware.async_params.print_warnings = + parse_bool(get_attribute_value(async_it, kPrintWarningsAttribute, kAsyncTag)); + } + } + catch (const std::exception & e) + { + throw std::runtime_error( + fmt::format(FMT_COMPILE("Error parsing {} tag: {}"), kAsyncTag, e.what())); + } + } + } else if (std::string(kJointTag) == ros2_control_child_it->Name()) { hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it)); @@ -700,7 +785,8 @@ HardwareInfo parse_resource_from_xml( } else { - throw std::runtime_error("invalid tag name " + std::string(ros2_control_child_it->Name())); + throw std::runtime_error( + fmt::format(FMT_COMPILE("invalid tag name '{}'"), ros2_control_child_it->Name())); } ros2_control_child_it = ros2_control_child_it->NextSiblingElement(); } @@ -890,12 +976,12 @@ std::vector parse_control_resources_from_urdf(const std::string & if (!doc.Parse(urdf.c_str()) && doc.Error()) { throw std::runtime_error( - "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); + fmt::format(FMT_COMPILE("invalid URDF passed in to robot parser: {}"), doc.ErrorStr())); } if (doc.Error()) { throw std::runtime_error( - "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); + fmt::format(FMT_COMPILE("invalid URDF passed in to robot parser: {}"), doc.ErrorStr())); } // Find robot or sdf tag @@ -920,7 +1006,7 @@ std::vector parse_control_resources_from_urdf(const std::string & if (!ros2_control_it) { - throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); + throw std::runtime_error(fmt::format(FMT_COMPILE("no {} tag"), kROS2ControlTag)); } std::vector hardware_info; @@ -944,21 +1030,24 @@ std::vector parse_control_resources_from_urdf(const std::string & auto urdf_joint = model.getJoint(joint.name); if (!urdf_joint) { - throw std::runtime_error("Joint '" + joint.name + "' not found in URDF"); + throw std::runtime_error( + fmt::format(FMT_COMPILE("Joint '{}' not found in URDF"), joint.name)); } if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) { throw std::runtime_error( - "Joint '" + joint.name + "' has no mimic information in the URDF."); + fmt::format(FMT_COMPILE("Joint '{}' has no mimic information in the URDF"), joint.name)); } if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) { if (joint.command_interfaces.size() > 0) { throw std::runtime_error( - "Joint '" + joint.name + - "' has mimic attribute not set to false: Activated mimic joints cannot have command " - "interfaces."); + fmt::format( + FMT_COMPILE( + "Joint '{}' has mimic attribute not set to false: Activated mimic joints cannot " + "have command interfaces."), + joint.name)); } auto find_joint = [&hw_info](const std::string & name) { @@ -967,7 +1056,8 @@ std::vector parse_control_resources_from_urdf(const std::string & [&name](const auto & j) { return j.name == name; }); if (it == hw_info.joints.end()) { - throw std::runtime_error("Mimic joint '" + name + "' not found in tag"); + throw std::runtime_error( + fmt::format(FMT_COMPILE("Mimic joint '{}' not found in tag"), name)); } return static_cast(std::distance(hw_info.joints.begin(), it)); }; @@ -983,9 +1073,10 @@ std::vector parse_control_resources_from_urdf(const std::string & if (urdf_joint->type == urdf::Joint::FIXED) { throw std::runtime_error( - "Joint '" + joint.name + - "' is of type 'fixed'. " - "Fixed joints do not make sense in ros2_control."); + fmt::format( + FMT_COMPILE( + "Joint '{}' is of type 'fixed'. Fixed joints do not make sense in ros2_control."), + joint.name)); } joint_limits::JointLimits limits; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/hardware_component.cpp similarity index 73% rename from hardware_interface/src/actuator.cpp rename to hardware_interface/src/hardware_component.cpp index b7f4a779a2..9d7a937e36 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/hardware_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2020-2021 ros2_control Development Team +// Copyright 2025 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. @@ -12,53 +12,47 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "hardware_interface/actuator.hpp" +#include "hardware_interface/hardware_component.hpp" #include #include #include #include -#include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_component_interface.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lifecycle_helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -// TODO(destogl): Add transition names also namespace hardware_interface { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} +HardwareComponent::HardwareComponent(std::unique_ptr impl) +: impl_(std::move(impl)) +{ +} -Actuator::Actuator(Actuator && other) noexcept +HardwareComponent::HardwareComponent(HardwareComponent && other) noexcept { - std::lock_guard lock(other.actuators_mutex_); + std::lock_guard lock(other.component_mutex_); impl_ = std::move(other.impl_); last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); } -const rclcpp_lifecycle::State & Actuator::initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -{ - return this->initialize(actuator_info, logger, clock_interface->get_clock()); -} - -const rclcpp_lifecycle::State & Actuator::initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) +const rclcpp_lifecycle::State & HardwareComponent::initialize( + const hardware_interface::HardwareComponentParams & params) { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(actuator_info, logger, clock)) + switch (impl_->init(params)) { case CallbackReturn::SUCCESS: impl_->set_lifecycle_state( @@ -77,11 +71,12 @@ const rclcpp_lifecycle::State & Actuator::initialize( return impl_->get_lifecycle_state(); } -const rclcpp_lifecycle::State & Actuator::configure() +const rclcpp_lifecycle::State & HardwareComponent::configure() { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + impl_->pause_async_operations(); switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -103,12 +98,13 @@ const rclcpp_lifecycle::State & Actuator::configure() return impl_->get_lifecycle_state(); } -const rclcpp_lifecycle::State & Actuator::cleanup() +const rclcpp_lifecycle::State & HardwareComponent::cleanup() { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); impl_->enable_introspection(false); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + impl_->pause_async_operations(); switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -126,14 +122,15 @@ const rclcpp_lifecycle::State & Actuator::cleanup() return impl_->get_lifecycle_state(); } -const rclcpp_lifecycle::State & Actuator::shutdown() +const rclcpp_lifecycle::State & HardwareComponent::shutdown() { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); impl_->enable_introspection(false); if ( impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + impl_->pause_async_operations(); switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -150,15 +147,19 @@ const rclcpp_lifecycle::State & Actuator::shutdown() return impl_->get_lifecycle_state(); } -const rclcpp_lifecycle::State & Actuator::activate() +const rclcpp_lifecycle::State & HardwareComponent::activate() { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); read_statistics_.reset_statistics(); - write_statistics_.reset_statistics(); + if (impl_->get_hardware_info().type != "sensor") + { + write_statistics_.reset_statistics(); + } if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + impl_->pause_async_operations(); impl_->prepare_for_activation(); switch (impl_->on_activate(impl_->get_lifecycle_state())) { @@ -181,12 +182,13 @@ const rclcpp_lifecycle::State & Actuator::activate() return impl_->get_lifecycle_state(); } -const rclcpp_lifecycle::State & Actuator::deactivate() +const rclcpp_lifecycle::State & HardwareComponent::deactivate() { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); impl_->enable_introspection(false); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + impl_->pause_async_operations(); switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -207,14 +209,15 @@ const rclcpp_lifecycle::State & Actuator::deactivate() return impl_->get_lifecycle_state(); } -const rclcpp_lifecycle::State & Actuator::error() +const rclcpp_lifecycle::State & HardwareComponent::error() { - std::unique_lock lock(actuators_mutex_); + std::unique_lock lock(component_mutex_); impl_->enable_introspection(false); if ( impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + impl_->pause_async_operations(); switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -234,11 +237,14 @@ const rclcpp_lifecycle::State & Actuator::error() return impl_->get_lifecycle_state(); } -std::vector Actuator::export_state_interfaces() +std::vector HardwareComponent::export_state_interfaces() { - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed +// BEGIN (Handle export change): for backward compatibility, can be removed if +// export_command_interfaces() method is removed +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::vector interfaces = impl_->export_state_interfaces(); +#pragma GCC diagnostic pop // END: for backward compatibility // If no StateInterfaces has been exported, this could mean: @@ -262,11 +268,14 @@ std::vector Actuator::export_state_interfaces() // END: for backward compatibility } -std::vector Actuator::export_command_interfaces() +std::vector HardwareComponent::export_command_interfaces() { - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed +// BEGIN (Handle export change): for backward compatibility, can be removed if +// export_command_interfaces() method is removed +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::vector interfaces = impl_->export_command_interfaces(); +#pragma GCC diagnostic pop // END: for backward compatibility // If no CommandInterface has been exported, this could mean: @@ -289,44 +298,47 @@ std::vector Actuator::export_command_interfaces() // END: for backward compatibility } -return_type Actuator::prepare_command_mode_switch( +return_type HardwareComponent::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { return impl_->prepare_command_mode_switch(start_interfaces, stop_interfaces); } -return_type Actuator::perform_command_mode_switch( +return_type HardwareComponent::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); } -const std::string & Actuator::get_name() const { return impl_->get_name(); } +const std::string & HardwareComponent::get_name() const { return impl_->get_name(); } -const std::string & Actuator::get_group_name() const { return impl_->get_group_name(); } +const std::string & HardwareComponent::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const +const rclcpp_lifecycle::State & HardwareComponent::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 & HardwareComponent::get_last_read_time() const { return last_read_cycle_time_; } -const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } +const rclcpp::Time & HardwareComponent::get_last_write_time() const +{ + return last_write_cycle_time_; +} -const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const +const HardwareComponentStatisticsCollector & HardwareComponent::get_read_statistics() const { return read_statistics_; } -const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const +const HardwareComponentStatisticsCollector & HardwareComponent::get_write_statistics() const { return write_statistics_; } -return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) +return_type HardwareComponent::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { @@ -346,12 +358,12 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p { if (trigger_result.execution_time.has_value()) { - read_statistics_.execution_time->AddMeasurement( + read_statistics_.execution_time->add_measurement( static_cast(trigger_result.execution_time.value().count()) / 1.e3); } if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) { - read_statistics_.periodicity->AddMeasurement( + read_statistics_.periodicity->add_measurement( 1.0 / (time - last_read_cycle_time_).seconds()); } last_read_cycle_time_ = time; @@ -361,16 +373,20 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return return_type::OK; } -return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) +return_type HardwareComponent::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + if (impl_->get_hardware_info().type == "sensor") + { + return return_type::OK; + } + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { last_write_cycle_time_ = time; return return_type::OK; } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + // only call write in the active state + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { const auto trigger_result = impl_->trigger_write(time, period); if (trigger_result.result == return_type::ERROR) @@ -381,12 +397,12 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & { if (trigger_result.execution_time.has_value()) { - write_statistics_.execution_time->AddMeasurement( + write_statistics_.execution_time->add_measurement( static_cast(trigger_result.execution_time.value().count()) / 1.e3); } if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) { - write_statistics_.periodicity->AddMeasurement( + write_statistics_.periodicity->add_measurement( 1.0 / (time - last_write_cycle_time_).seconds()); } last_write_cycle_time_ = time; @@ -396,5 +412,5 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & return return_type::OK; } -std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } +std::recursive_mutex & HardwareComponent::get_mutex() { return component_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp index f9bdbedb57..d5087976f1 100644 --- a/hardware_interface/src/lexical_casts.cpp +++ b/hardware_interface/src/lexical_casts.cpp @@ -12,9 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include -#include +#include +#include +#include #include "hardware_interface/lexical_casts.hpp" @@ -48,6 +52,7 @@ std::optional stod(const std::string & s) #endif } } // namespace impl + double stod(const std::string & s) { if (const auto result = impl::stod(s)) @@ -56,8 +61,38 @@ double stod(const std::string & s) } throw std::invalid_argument("Failed converting string to real number"); } + +std::string to_lower_case(const std::string & string) +{ + std::string lower_case_string = string; + std::transform( + lower_case_string.begin(), lower_case_string.end(), lower_case_string.begin(), + [](unsigned char c) { return std::tolower(c); }); + return lower_case_string; +} + bool parse_bool(const std::string & bool_string) { - return bool_string == "true" || bool_string == "True"; + // Copy input to temp and make lowercase + std::string temp = to_lower_case(bool_string); + + if (temp == "true") + { + return true; + } + if (temp == "false") + { + return false; + } + // If input is not "true" or "false" (any casing), throw or handle as error + throw std::invalid_argument( + "Input string : '" + bool_string + + "' is not a valid boolean value. Expected 'true' or 'false'."); +} + +std::vector parse_string_array(const std::string & string_array_string) +{ + return parse_array(string_array_string); } + } // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 2b1d635c97..6fe122e9e8 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -17,6 +17,7 @@ #include "mock_components/generic_system.hpp" #include +#include #include #include #include @@ -30,34 +31,14 @@ namespace mock_components { -CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) +CallbackReturn GenericSystem::on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } - auto populate_non_standard_interfaces = - [this](auto interface_list, auto & non_standard_interfaces) - { - for (const auto & interface : interface_list) - { - // add to list if non-standard interface - if ( - std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) == - standard_interfaces_.end()) - { - if ( - std::find( - non_standard_interfaces.begin(), non_standard_interfaces.end(), interface.name) == - non_standard_interfaces.end()) - { - non_standard_interfaces.emplace_back(interface.name); - } - } - } - }; - // check if to create mock command interface for sensor auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands"); if (it != get_hardware_info().hardware_parameters.end()) @@ -83,7 +64,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // check if there is parameter that disables commands // this way we simulate disconnected driver it = get_hardware_info().hardware_parameters.find("disable_commands"); - if (it != info.hardware_parameters.end()) + if (it != get_hardware_info().hardware_parameters.end()) { command_propagation_disabled_ = hardware_interface::parse_bool(it->second); } @@ -94,7 +75,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // check if there is parameter that enables dynamic calculation it = get_hardware_info().hardware_parameters.find("calculate_dynamics"); - if (it != info.hardware_parameters.end()) + if (it != get_hardware_info().hardware_parameters.end()) { calculate_dynamics_ = hardware_interface::parse_bool(it->second); } @@ -102,6 +83,13 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i { calculate_dynamics_ = false; } + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position, + // velocity, and acceleration interface + // Create a subvector of standard_interfaces_ with the given indices + for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i) + { + skip_interfaces_.push_back(standard_interfaces_[i]); + } // process parameters about state following position_state_following_offset_ = 0.0; @@ -117,51 +105,50 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // 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 - initialize_storage_vectors( - joint_commands_, joint_states_, standard_interfaces_, get_hardware_info().joints); - // set all values without initial values to 0 - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) + + // search for non-standard joint interfaces + std::vector other_interfaces; + auto populate_non_standard_interfaces = + [this](auto interface_list, auto & non_standard_interfaces) { - for (auto j = 0u; j < standard_interfaces_.size(); j++) + for (const auto & interface : interface_list) { - if (std::isnan(joint_states_[j][i])) + // add to list if non-standard interface + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) == + standard_interfaces_.end()) { - joint_states_[j][i] = 0.0; + // and does not exist yet + if ( + std::find( + non_standard_interfaces.begin(), non_standard_interfaces.end(), interface.name) == + non_standard_interfaces.end()) + { + non_standard_interfaces.emplace_back(interface.name); + } } } - } - - // search for non-standard joint interfaces + }; for (const auto & joint : get_hardware_info().joints) { - // populate non-standard command interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); + // populate non-standard command interfaces to other_interfaces + populate_non_standard_interfaces(joint.command_interfaces, other_interfaces); - // populate non-standard state interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_); + // populate non-standard state interfaces to other_interfaces + populate_non_standard_interfaces(joint.state_interfaces, other_interfaces); } - // Initialize storage for non-standard interfaces - initialize_storage_vectors( - other_commands_, other_states_, other_interfaces_, get_hardware_info().joints); - // when following offset is used on custom interface then find its index if (!custom_interface_with_following_offset_.empty()) { - auto if_it = std::find( - other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); - if (if_it != other_interfaces_.end()) + if ( + std::find( + other_interfaces.begin(), other_interfaces.end(), + custom_interface_with_following_offset_) != other_interfaces.end()) { - index_custom_interface_with_following_offset_ = - 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(), - index_custom_interface_with_following_offset_); + get_logger(), "Custom interface with following offset '%s' found.", + custom_interface_with_following_offset_.c_str()); } else { @@ -172,154 +159,26 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : get_hardware_info().sensors) - { - for (const auto & interface : sensor.state_interfaces) - { - if ( - std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) == - sensor_interfaces_.end()) - { - sensor_interfaces_.emplace_back(interface.name); - } - } - } - initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, get_hardware_info().sensors); - - // search for gpio interfaces - for (const auto & gpio : get_hardware_info().gpios) - { - // populate non-standard command interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); - - // populate non-standard state interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_); - } - - // Mock gpio command interfaces - if (use_mock_gpio_command_interfaces_) - { - initialize_storage_vectors( - gpio_mock_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); - } - // Real gpio command interfaces - else - { - initialize_storage_vectors( - gpio_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); - } - return CallbackReturn::SUCCESS; } -std::vector GenericSystem::export_state_interfaces() +std::vector +GenericSystem::export_unlisted_command_interface_descriptions() { - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto & joint = get_hardware_info().joints[i]; - for (const auto & interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - - // Sensor state interfaces - if (!populate_interfaces( - get_hardware_info().sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - }; - - // GPIO state interfaces - if (!populate_interfaces( - get_hardware_info().gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) - { - throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); - } - - return state_interfaces; -} - -std::vector GenericSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - // Joints' state interfaces - for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) - { - const auto & joint = get_hardware_info().joints[i]; - for (const auto & interface : joint.command_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - // Set position control mode per default - joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); - + std::vector command_interface_descriptions; // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { - if (!populate_interfaces( - get_hardware_info().sensors, sensor_interfaces_, sensor_mock_commands_, - command_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - } + populate_interfaces(get_hardware_info().sensors, command_interface_descriptions); } // Mock gpio command interfaces (consider all state interfaces for command interfaces) if (use_mock_gpio_command_interfaces_) { - if (!populate_interfaces( - get_hardware_info().gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, - true)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } - } - // GPIO command interfaces (real command interfaces) - else - { - if (!populate_interfaces( - get_hardware_info().gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } + populate_interfaces(get_hardware_info().gpios, command_interface_descriptions); } - return command_interfaces; + return command_interface_descriptions; } return_type GenericSystem::prepare_command_mode_switch( @@ -361,26 +220,10 @@ return_type GenericSystem::prepare_command_mode_switch( } if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { - if (!calculate_dynamics_) - { - RCLCPP_WARN( - get_logger(), - "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " - "might lead to wrong feedback and unexpected behavior.", - info.joints[joint_index].name.c_str()); - } joint_found_in_x_requests_[joint_index] += 1; } if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { - if (!calculate_dynamics_) - { - RCLCPP_WARN( - get_logger(), - "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " - "this might lead to wrong feedback and unexpected behavior.", - info.joints[joint_index].name.c_str()); - } joint_found_in_x_requests_[joint_index] += 1; } } @@ -439,7 +282,6 @@ return_type GenericSystem::perform_command_mode_switch( { const size_t joint_index = static_cast(std::distance(info.joints.begin(), joint_it_found)); - if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; @@ -458,6 +300,35 @@ return_type GenericSystem::perform_command_mode_switch( return hardware_interface::return_type::OK; } +hardware_interface::CallbackReturn GenericSystem::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set position control mode per default + // This will be populated by perform_command_mode_switch + joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn GenericSystem::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (const auto & joint_state : joint_state_interfaces_) + { + const std::string & name = joint_state.second.get_name(); + // initial values are set from the URDF. only apply offset + if (joint_state.second.get_data_type() == hardware_interface::HandleDataType::DOUBLE) + { + if ( + joint_state.second.get_interface_name() == hardware_interface::HW_IF_POSITION && + custom_interface_with_following_offset_.empty()) + { + set_state(name, get_state(name) + position_state_following_offset_); + } + } + } + return hardware_interface::CallbackReturn::SUCCESS; +} + return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_propagation_disabled_) @@ -466,233 +337,298 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } - auto mirror_command_to_state = - [](auto & states_, auto commands_, size_t start_index = 0) -> return_type + auto mirror_command_to_state = [this](const auto & name, const auto & data_type) -> return_type { - for (size_t i = start_index; i < states_.size(); ++i) + switch (data_type) { - for (size_t j = 0; j < states_[i].size(); ++j) + case hardware_interface::HandleDataType::DOUBLE: { - if (!std::isnan(commands_[i][j])) + auto cmd = get_command(name); + if (std::isinf(cmd)) { - states_[i][j] = commands_[i][j]; + return return_type::ERROR; } - if (std::isinf(commands_[i][j])) + else if (std::isfinite(cmd)) { - return return_type::ERROR; + set_state(name, cmd); } + else + { + // NaN - do nothing. Command might not be set yet + } + break; + } + case hardware_interface::HandleDataType::BOOL: + { + set_state(name, get_command(name)); + break; } + default: + { + } + // not handling other types } return return_type::OK; }; - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + for (size_t j = 0; j < get_hardware_info().joints.size(); ++j) { if (calculate_dynamics_) { + std::array joint_state_values_ = { + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; + std::array joint_command_values_ = { + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; + const auto & joint_name = get_hardware_info().joints[j].name; + for (size_t i = 0; i < 3; ++i) + { + const auto full_name = joint_name + "/" + standard_interfaces_[i]; + if (has_command(full_name)) + { + joint_command_values_[i] = get_command(full_name); + } + if (has_state(full_name)) + { + joint_state_values_[i] = get_state(full_name); + } + } + switch (joint_control_mode_[j]) { case ACCELERATION_INTERFACE_INDEX: { - // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - joint_states_[VELOCITY_INTERFACE_INDEX][j] += - joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + if (std::isnan(joint_state_values_[VELOCITY_INTERFACE_INDEX])) + { + joint_state_values_[VELOCITY_INTERFACE_INDEX] = 0.0; + } - if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + if (std::isfinite(joint_command_values_[ACCELERATION_INTERFACE_INDEX])) { - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = + joint_command_values_[ACCELERATION_INTERFACE_INDEX]; } + // currently we do backward Euler integration + joint_state_values_[VELOCITY_INTERFACE_INDEX] += + std::isnan(joint_state_values_[ACCELERATION_INTERFACE_INDEX]) + ? 0.0 + : joint_state_values_[ACCELERATION_INTERFACE_INDEX] * period.seconds(); + joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only + std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX]) + ? joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + : 0.0 + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0); break; } case VELOCITY_INTERFACE_INDEX: { - // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + if (std::isfinite(joint_command_values_[VELOCITY_INTERFACE_INDEX])) { - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const double old_velocity = std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX]) + ? joint_state_values_[VELOCITY_INTERFACE_INDEX] + : 0.0; + joint_state_values_[VELOCITY_INTERFACE_INDEX] = + joint_command_values_[VELOCITY_INTERFACE_INDEX]; + + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = + (joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds(); } + // currently we do backward Euler integration + joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only + std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX]) + ? joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + : 0.0 + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0); break; } case POSITION_INTERFACE_INDEX: { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (std::isfinite(joint_command_values_[POSITION_INTERFACE_INDEX])) { - const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX]; + const double old_velocity = std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX]) + ? joint_state_values_[VELOCITY_INTERFACE_INDEX] + : 0.0; - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + + joint_state_values_[POSITION_INTERFACE_INDEX] = // apply offset to positions only + joint_command_values_[POSITION_INTERFACE_INDEX] + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + joint_state_values_[VELOCITY_INTERFACE_INDEX] = + (joint_state_values_[POSITION_INTERFACE_INDEX] - old_position) / period.seconds(); - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = + (joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds(); } break; } } + // mirror them back + for (size_t i = 0; i < standard_interfaces_.size(); ++i) + { + if ( + std::isfinite(joint_state_values_[i]) && + has_state(joint_name + "/" + standard_interfaces_[i])) + { + set_state(joint_name + "/" + standard_interfaces_[i], joint_state_values_[i]); + } + } } else { - for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) + for (const auto & joint_command : joint_commands_) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][k])) + if (joint_command.get()->get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_states_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][k] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); + const std::string & name = joint_command.get()->get_name(); + if (has_state(name)) + { + set_state( + name, get_command(name) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); + } } } } } - // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, - // velocity, and acceleration interface - if ( - mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) != - return_type::OK) + // do loopback on all other interfaces + for (const auto & joint_state : joint_states_) { - return return_type::ERROR; + if ( + std::find( + skip_interfaces_.begin(), skip_interfaces_.end(), + joint_state.get()->get_interface_name()) != skip_interfaces_.end()) + { + continue; + } + const std::string & full_interface_name = joint_state.get()->get_name(); + if (has_command(full_interface_name)) + { + if ( + mirror_command_to_state(full_interface_name, joint_state.get()->get_data_type()) != + return_type::OK) + { + return return_type::ERROR; + } + } + if (custom_interface_with_following_offset_ == joint_state.get()->get_interface_name()) + { + const auto cmd = + get_command( + joint_state.get()->get_prefix_name() + "/" + hardware_interface::HW_IF_POSITION) + + position_state_following_offset_; + set_state(full_interface_name, cmd); + } } + // Update mimic joints + const auto & joints = get_hardware_info().joints; for (const auto & mimic_joint : get_hardware_info().mimic_joints) { - for (auto i = 0u; i < joint_states_.size(); ++i) + const auto & mimic_joint_name = joints.at(mimic_joint.joint_index).name; + const auto & mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION)) { - joint_states_[i][mimic_joint.joint_index] = + set_state( + mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, mimic_joint.offset + - mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; + mimic_joint.multiplier * + get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); + } + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)) + { + set_state( + mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, + mimic_joint.multiplier * + get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); + } + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)) + { + set_state( + mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, + mimic_joint.multiplier * + get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); } } - for (size_t i = 0; i < other_states_.size(); ++i) + if (use_mock_sensor_command_interfaces_) { - for (size_t j = 0; j < other_states_[i].size(); ++j) + // do loopback on all sensor interfaces as we have exported them all + for (const auto & sensor_state : sensor_states_) { - if ( - i == index_custom_interface_with_following_offset_ && - !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + const std::string & name = sensor_state.get()->get_name(); + if (mirror_command_to_state(name, sensor_state.get()->get_data_type()) != return_type::OK) { - other_states_[i][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; - } - else if (!std::isnan(other_commands_[i][j])) - { - other_states_[i][j] = other_commands_[i][j]; + return return_type::ERROR; } } } - if (use_mock_sensor_command_interfaces_) - { - mirror_command_to_state(sensor_states_, sensor_mock_commands_); - } - if (use_mock_gpio_command_interfaces_) { - mirror_command_to_state(gpio_states_, gpio_mock_commands_); + // do loopback on all gpio interfaces as we have exported them all + // commands are created for all state interfaces, but in unlisted_commands_ + for (const auto & gpio_state : gpio_states_) + { + const std::string & name = gpio_state.get()->get_name(); + if (mirror_command_to_state(name, gpio_state.get()->get_data_type()) != return_type::OK) + { + return return_type::ERROR; + } + } } else { - // do loopback on all gpio interfaces - mirror_command_to_state(gpio_states_, gpio_commands_); - } - - return return_type::OK; -} - -// Private methods -template -bool GenericSystem::get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces) -{ - auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); - if (it != interface_list.end()) - { - auto j = static_cast(std::distance(interface_list.begin(), it)); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - return false; -} - -void GenericSystem::initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos) -{ - // Initialize storage for all joints, regardless of their existence - commands.resize(interfaces.size()); - states.resize(interfaces.size()); - for (auto i = 0u; i < interfaces.size(); i++) - { - commands[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - states[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - } - - // Initialize with values from URDF - for (auto i = 0u; i < component_infos.size(); i++) - { - const auto & component = component_infos[i]; - for (const auto & interface : component.state_interfaces) + // do loopback on all gpio interfaces, where they exist + for (const auto & gpio_command : gpio_commands_) { - auto it = std::find(interfaces.begin(), interfaces.end(), interface.name); - - // If interface name is found in the interfaces list - if (it != interfaces.end()) + const std::string & name = gpio_command.get()->get_name(); + if (has_state(name)) { - auto index = static_cast(std::distance(interfaces.begin(), it)); - - // Check the initial_value param is used - if (!interface.initial_value.empty()) + if (mirror_command_to_state(name, gpio_command.get()->get_data_type()) != return_type::OK) { - states[index][i] = hardware_interface::stod(interface.initial_value); + return return_type::ERROR; } } } } + + return return_type::OK; } -template +// Private methods bool GenericSystem::populate_interfaces( const std::vector & components, - std::vector & interface_names, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces) + std::vector & command_interface_descriptions) const { - for (auto i = 0u; i < components.size(); i++) + for (const auto & component : components) { - const auto & component = components[i]; - const auto interfaces = - (using_state_interfaces) ? component.state_interfaces : component.command_interfaces; - for (const auto & interface : interfaces) + for (const auto & state_interface : component.state_interfaces) { - if (!get_interface( - component.name, interface_names, interface.name, i, storage, target_interfaces)) + // add to state interface to command interface list if not already there + if ( + std::find_if( + command_interface_descriptions.begin(), command_interface_descriptions.end(), + [&component, &state_interface](const auto & desc) + { return (desc.get_name() == (component.name + "/" + state_interface.name)); }) == + command_interface_descriptions.end() && + std::find_if( + component.command_interfaces.begin(), component.command_interfaces.end(), + [&component, &state_interface](const auto & cmd_if) + { + return ( + (component.name + "/" + cmd_if.name) == + (component.name + "/" + state_interface.name)); + }) == component.command_interfaces.end()) { - return false; + hardware_interface::InterfaceDescription description(component.name, state_interface); + command_interface_descriptions.push_back(description); } } } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index eb67f9fec1..39290551a2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -14,6 +14,8 @@ #include "hardware_interface/resource_manager.hpp" +#include + #include #include #include @@ -87,7 +89,37 @@ std::string interfaces_to_string( } ss << "]" << std::endl; return ss.str(); -}; +} + +void find_common_hardware_interfaces( + const std::vector & hw_command_itfs, + const std::vector & start_stop_interfaces_list, + std::vector & hw_interfaces) +{ + hw_interfaces.clear(); + + // decide which input vector is shorter. + const auto & shorter_vec = hw_command_itfs.size() < start_stop_interfaces_list.size() + ? hw_command_itfs + : start_stop_interfaces_list; + const auto & longer_vec = + &shorter_vec == &hw_command_itfs ? start_stop_interfaces_list : hw_command_itfs; + + // reserve exactly the worst-case result size (all of the smaller one). + hw_interfaces.reserve(shorter_vec.size()); + + // build a hash set from the smaller vector. + std::unordered_set lookup(shorter_vec.begin(), shorter_vec.end()); + + // iterate through the larger vector; test membership in constant time. + for (const auto & name : longer_vec) + { + if (lookup.find(name) != lookup.end()) + { + hw_interfaces.push_back(name); + } + } +} class ResourceStorage { @@ -203,39 +235,48 @@ class ResourceStorage } template - bool initialize_hardware(const HardwareInfo & hardware_info, HardwareT & hardware) - { - RCLCPP_INFO(get_logger(), "Initialize hardware '%s' ", hardware_info.name.c_str()); + bool initialize_hardware( + const hardware_interface::HardwareComponentParams & params, HardwareT & hardware) + { + hardware_interface::HardwareComponentParams component_params; + component_params.hardware_info = params.hardware_info; + component_params.clock = rm_clock_; + component_params.logger = rm_logger_; + component_params.executor = params.executor; + component_params.node_namespace = params.node_namespace; + RCLCPP_INFO( + get_logger(), "Initialize hardware '%s' ", component_params.hardware_info.name.c_str()); bool result = false; try { - const rclcpp_lifecycle::State new_state = - hardware.initialize(hardware_info, rm_logger_, rm_clock_); + const rclcpp_lifecycle::State new_state = hardware.initialize(component_params); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; if (result) { RCLCPP_INFO( - get_logger(), "Successful initialization of hardware '%s'", hardware_info.name.c_str()); + get_logger(), "Successful initialization of hardware '%s'", + component_params.hardware_info.name.c_str()); } else { RCLCPP_ERROR( - get_logger(), "Failed to initialize hardware '%s'", hardware_info.name.c_str()); + get_logger(), "Failed to initialize hardware '%s'", + component_params.hardware_info.name.c_str()); } } catch (const std::exception & ex) { RCLCPP_ERROR( get_logger(), "Exception of type : %s occurred while initializing hardware '%s': %s", - typeid(ex).name(), hardware_info.name.c_str(), ex.what()); + typeid(ex).name(), component_params.hardware_info.name.c_str(), ex.what()); } catch (...) { RCLCPP_ERROR( get_logger(), "Unknown exception occurred while initializing hardware '%s'", - hardware_info.name.c_str()); + component_params.hardware_info.name.c_str()); } return result; @@ -617,6 +658,7 @@ class ResourceStorage { auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; RCLCPP_WARN_EXPRESSION( get_logger(), interface_names.empty(), @@ -633,9 +675,11 @@ class ResourceStorage std::make_pair(command_interface->get_name(), command_interface)); if (!success) { - std::string msg( - "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + - command_interface->get_name() + "]"); + const std::string msg = fmt::format( + FMT_COMPILE( + "ResourceStorage: Tried to insert CommandInterface with already existing key. " + "Insert[{}]"), + command_interface->get_name()); throw std::runtime_error(msg); } command_interface->registerIntrospection(); @@ -650,9 +694,11 @@ class ResourceStorage std::make_pair(key, std::make_shared(std::move(command_interface)))); if (!success) { - std::string msg( - "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + - key + "]"); + const std::string msg = fmt::format( + FMT_COMPILE( + "ResourceStorage: Tried to insert CommandInterface with already existing key. " + "Insert[{}]"), + key); throw std::runtime_error(msg); } } @@ -666,6 +712,8 @@ class ResourceStorage auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + start_interfaces_buffer_.reserve(start_interfaces_buffer_.capacity() + interfaces.size()); + stop_interfaces_buffer_.reserve(stop_interfaces_buffer_.capacity() + interfaces.size()); // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) @@ -692,14 +740,16 @@ class ResourceStorage for (const auto & [joint_name, limits] : hw_info.limits) { std::vector soft_limits; + hard_joint_limits_.insert({joint_name, limits}); const std::vector hard_limits{limits}; joint_limits::JointInterfacesCommandLimiterData data; - data.joint_name = joint_name; + data.set_joint_name(joint_name); limiters_data_.insert({joint_name, data}); // If the joint limits is found in the softlimits, then extract it if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end()) { soft_limits = {hw_info.soft_limits.at(joint_name)}; + soft_joint_limits_.insert({joint_name, hw_info.soft_limits.at(joint_name)}); RCLCPP_INFO( get_logger(), "Using SoftJointLimiter for joint '%s' in hardware '%s' : '%s'", joint_name.c_str(), hw_info.name.c_str(), soft_limits[0].to_string().c_str()); @@ -744,7 +794,8 @@ class ResourceStorage const auto fill_interface_data = [&](const std::string & interface_type, std::optional & value) { - const std::string interface_name = joint_name + "/" + interface_type; + const std::string interface_name = + fmt::format(FMT_COMPILE("{}/{}"), joint_name, interface_type); if (interface_map.find(interface_name) != interface_map.end()) { // If the command interface is not claimed, then the value is not set (or) if the @@ -778,7 +829,8 @@ class ResourceStorage const auto set_interface_command = [&](const std::string & interface_type, const std::optional & data) { - const std::string interface_name = limited_command.joint_name + "/" + interface_type; + const std::string interface_name = + fmt::format(FMT_COMPILE("{}/{}"), limited_command.joint_name, interface_type); if (data.has_value() && interface_map.find(interface_name) != interface_map.end()) { auto itf_handle = interface_map.at(interface_name); @@ -832,9 +884,10 @@ class ResourceStorage const auto [it, success] = state_interface_map_.emplace(interface_name, interface); if (!success) { - std::string msg( - "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + - interface->get_name() + "]"); + const std::string msg = fmt::format( + FMT_COMPILE( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[{}]"), + interface->get_name()); throw std::runtime_error(msg); } interface->registerIntrospection(); @@ -976,7 +1029,7 @@ class ResourceStorage { is_limited = false; joint_limits::JointInterfacesCommandLimiterData data; - data.joint_name = joint_name; + data.set_joint_name(joint_name); update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual); if (interface_name == hardware_interface::HW_IF_POSITION) { @@ -1058,16 +1111,16 @@ class ResourceStorage } } - // TODO(destogl): Propagate "false" up, if happens in initialize_hardware - bool load_and_initialize_actuator(const HardwareInfo & hardware_info) + bool load_and_initialize_actuator(const hardware_interface::HardwareComponentParams & params) { auto load_and_init_actuators = [&](auto & container) { - if (!load_hardware(hardware_info, actuator_loader_, container)) + if (!load_hardware( + params.hardware_info, actuator_loader_, container)) { return false; } - if (initialize_hardware(hardware_info, container.back())) + if (initialize_hardware(params, container.back())) { import_state_interfaces(container.back()); import_command_interfaces(container.back()); @@ -1076,7 +1129,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "Actuator hardware component '%s' from plugin '%s' failed to initialize.", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + params.hardware_info.name.c_str(), params.hardware_info.hardware_plugin_name.c_str()); return false; } return true; @@ -1084,15 +1137,15 @@ class ResourceStorage return load_and_init_actuators(actuators_); } - bool load_and_initialize_sensor(const HardwareInfo & hardware_info) + bool load_and_initialize_sensor(const hardware_interface::HardwareComponentParams & params) { auto load_and_init_sensors = [&](auto & container) { - if (!load_hardware(hardware_info, sensor_loader_, container)) + if (!load_hardware(params.hardware_info, sensor_loader_, container)) { return false; } - if (initialize_hardware(hardware_info, container.back())) + if (initialize_hardware(params, container.back())) { import_state_interfaces(container.back()); } @@ -1100,7 +1153,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "Sensor hardware component '%s' from plugin '%s' failed to initialize.", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + params.hardware_info.name.c_str(), params.hardware_info.hardware_plugin_name.c_str()); return false; } return true; @@ -1109,15 +1162,15 @@ class ResourceStorage return load_and_init_sensors(sensors_); } - bool load_and_initialize_system(const HardwareInfo & hardware_info) + bool load_and_initialize_system(const hardware_interface::HardwareComponentParams & params) { auto load_and_init_systems = [&](auto & container) { - if (!load_hardware(hardware_info, system_loader_, container)) + if (!load_hardware(params.hardware_info, system_loader_, container)) { return false; } - if (initialize_hardware(hardware_info, container.back())) + if (initialize_hardware(params, container.back())) { import_state_interfaces(container.back()); import_command_interfaces(container.back()); @@ -1126,7 +1179,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "System hardware component '%s' from plugin '%s' failed to initialize.", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + params.hardware_info.name.c_str(), params.hardware_info.hardware_plugin_name.c_str()); return false; } return true; @@ -1135,12 +1188,13 @@ class ResourceStorage } void initialize_actuator( - std::unique_ptr actuator, const HardwareInfo & hardware_info) + std::unique_ptr actuator, + const hardware_interface::HardwareComponentParams & params) { auto init_actuators = [&](auto & container) { container.emplace_back(Actuator(std::move(actuator))); - if (initialize_hardware(hardware_info, container.back())) + if (initialize_hardware(params, container.back())) { import_state_interfaces(container.back()); import_command_interfaces(container.back()); @@ -1149,7 +1203,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "Actuator hardware component '%s' from plugin '%s' failed to initialize.", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + params.hardware_info.name.c_str(), params.hardware_info.hardware_plugin_name.c_str()); } }; @@ -1157,12 +1211,13 @@ class ResourceStorage } void initialize_sensor( - std::unique_ptr sensor, const HardwareInfo & hardware_info) + std::unique_ptr sensor, + const hardware_interface::HardwareComponentParams & params) { auto init_sensors = [&](auto & container) { container.emplace_back(Sensor(std::move(sensor))); - if (initialize_hardware(hardware_info, container.back())) + if (initialize_hardware(params, container.back())) { import_state_interfaces(container.back()); } @@ -1170,7 +1225,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "Sensor hardware component '%s' from plugin '%s' failed to initialize.", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + params.hardware_info.name.c_str(), params.hardware_info.hardware_plugin_name.c_str()); } }; @@ -1178,12 +1233,13 @@ class ResourceStorage } void initialize_system( - std::unique_ptr system, const HardwareInfo & hardware_info) + std::unique_ptr system, + const hardware_interface::HardwareComponentParams & params) { auto init_systems = [&](auto & container) { container.emplace_back(System(std::move(system))); - if (initialize_hardware(hardware_info, container.back())) + if (initialize_hardware(params, container.back())) { import_state_interfaces(container.back()); import_command_interfaces(container.back()); @@ -1192,7 +1248,7 @@ class ResourceStorage { RCLCPP_WARN( get_logger(), "System hardware component '%s' from plugin '%s' failed to initialize.", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + params.hardware_info.name.c_str(), params.hardware_info.hardware_plugin_name.c_str()); } }; @@ -1294,9 +1350,17 @@ class ResourceStorage std::string robot_description_; + // Unordered map of the hard and soft limits for the joints + std::unordered_map hard_joint_limits_; + std::unordered_map soft_joint_limits_; + /// The callback to be called when a component state is switched std::function on_component_state_switch_callback_ = nullptr; + // To be used with the prepare and perform command switch for the hardware components + std::vector start_interfaces_buffer_; + std::vector stop_interfaces_buffer_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1305,12 +1369,13 @@ class ResourceStorage ResourceManager::ResourceManager( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) -: resource_storage_(std::make_unique(clock_interface, logger_interface)) +: ResourceManager( + constructParams(clock_interface->get_clock(), logger_interface->get_logger()), false) { } ResourceManager::ResourceManager(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger) -: resource_storage_(std::make_unique(clock, logger)) +: ResourceManager(constructParams(clock, logger), false) { } @@ -1320,38 +1385,70 @@ ResourceManager::ResourceManager( const std::string & urdf, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, bool activate_all, const unsigned int update_rate) -: resource_storage_(std::make_unique(clock_interface, logger_interface)) +: ResourceManager( + constructParams( + clock_interface->get_clock(), logger_interface->get_logger(), urdf, activate_all, + update_rate), + true) { - resource_storage_->robot_description_ = urdf; - load_and_initialize_components(urdf, update_rate); - - if (activate_all) - { - for (auto const & hw_info : resource_storage_->hardware_info_map_) - { - using lifecycle_msgs::msg::State; - rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); - set_component_state(hw_info.first, state); - } - } } ResourceManager::ResourceManager( const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, bool activate_all, const unsigned int update_rate) -: resource_storage_(std::make_unique(clock, logger)) +: ResourceManager(constructParams(clock, logger, urdf, activate_all, update_rate), true) { - load_and_initialize_components(urdf, update_rate); +} - if (activate_all) - { - for (auto const & hw_info : resource_storage_->hardware_info_map_) +ResourceManager::ResourceManager( + const hardware_interface::ResourceManagerParams & params, bool load) +: resource_storage_(std::make_unique(params.clock, params.logger)) +{ + RCLCPP_WARN_EXPRESSION( + params.logger, params.allow_controller_activation_with_inactive_hardware, + "The parameter 'allow_controller_activation_with_inactive_hardware' is set to true. It is " + "recommended to use the settings to false in order to avoid controllers to use inactive " + "hardware components and to avoid any unexpected behavior. This feature might be removed in " + "future releases and will be defaulted to false."); + RCLCPP_WARN_EXPRESSION( + params.logger, !params.return_failed_hardware_names_on_return_deactivate_write_cycle_, + "The parameter 'deactivate_controllers_on_hardware_self_deactivate' is set to false. It is " + "recommended to use the settings to true in order to avoid controllers to use inactive " + "hardware components and to avoid any unexpected behavior. This feature might be removed in " + "future releases and will be defaulted to true."); + allow_controller_activation_with_inactive_hardware_ = + params.allow_controller_activation_with_inactive_hardware; + return_failed_hardware_names_on_return_deactivate_write_cycle_ = + params.return_failed_hardware_names_on_return_deactivate_write_cycle_; + + try + { + if (load && !load_and_initialize_components(params)) + { + RCLCPP_WARN( + get_logger(), + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + return; + } + if (params.activate_all) { - using lifecycle_msgs::msg::State; - rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); - set_component_state(hw_info.first, state); + for (auto const & hw_info : resource_storage_->hardware_info_map_) + { + using lifecycle_msgs::msg::State; + rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + set_component_state(hw_info.first, state); + } } } + catch (const std::exception &e) + { + // Other possible errors when loading components + RCLCPP_ERROR( + get_logger(), + "Exception caught while loading and initializing components: %s", e.what()); + return; + } } bool ResourceManager::shutdown_components() @@ -1372,25 +1469,27 @@ bool ResourceManager::shutdown_components() // CM API: Called in "callback/slow"-thread bool ResourceManager::load_and_initialize_components( - const std::string & urdf, const unsigned int update_rate) + const hardware_interface::ResourceManagerParams & params) { - components_are_loaded_and_initialized_ = true; + resource_storage_->robot_description_ = params.robot_description; + resource_storage_->cm_update_rate_ = params.update_rate; - resource_storage_->robot_description_ = urdf; - resource_storage_->cm_update_rate_ = update_rate; - - auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + auto hardware_info = + hardware_interface::parse_control_resources_from_urdf(params.robot_description); // 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; + hw.rw_rate = + (hw.rw_rate == 0 || hw.rw_rate > params.update_rate) ? params.update_rate : hw.rw_rate; } const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; + components_are_loaded_and_initialized_ = true; std::lock_guard resource_guard(resources_lock_); + std::lock_guard limiters_guard(joint_limiters_lock_); for (const auto & individual_hardware_info : hardware_info) { // Check for identical names @@ -1406,11 +1505,17 @@ bool ResourceManager::load_and_initialize_components( components_are_loaded_and_initialized_ = false; break; } + hardware_interface::HardwareComponentParams interface_params; + interface_params.hardware_info = individual_hardware_info; + interface_params.executor = params.executor; + interface_params.clock = params.clock; + interface_params.logger = params.logger; + interface_params.node_namespace = params.node_namespace; if (individual_hardware_info.type == actuator_type) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - if (!resource_storage_->load_and_initialize_actuator(individual_hardware_info)) + if (!resource_storage_->load_and_initialize_actuator(interface_params)) { components_are_loaded_and_initialized_ = false; break; @@ -1419,7 +1524,7 @@ bool ResourceManager::load_and_initialize_components( if (individual_hardware_info.type == sensor_type) { std::lock_guard guard(resource_interfaces_lock_); - if (!resource_storage_->load_and_initialize_sensor(individual_hardware_info)) + if (!resource_storage_->load_and_initialize_sensor(interface_params)) { components_are_loaded_and_initialized_ = false; break; @@ -1428,7 +1533,7 @@ bool ResourceManager::load_and_initialize_components( if (individual_hardware_info.type == system_type) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - if (!resource_storage_->load_and_initialize_system(individual_hardware_info)) + if (!resource_storage_->load_and_initialize_system(interface_params)) { components_are_loaded_and_initialized_ = false; break; @@ -1471,7 +1576,8 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & { if (!state_interface_is_available(key)) { - throw std::runtime_error(std::string("State interface with key '") + key + "' does not exist"); + throw std::runtime_error( + fmt::format(FMT_COMPILE("State interface with key '{}' does not exist"), key)); } std::lock_guard guard(resource_interfaces_lock_); @@ -1507,6 +1613,21 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +std::string ResourceManager::get_state_interface_data_type(const std::string & name) const +{ + std::lock_guard guard(resource_interfaces_lock_); + auto it = resource_storage_->state_interface_map_.find(name); + if (it != resource_storage_->state_interface_map_.end()) + { + return it->second->get_data_type().to_string(); + } + else + { + throw std::runtime_error( + std::string("State interface with key '") + name + std::string("' does not exist")); + } +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) @@ -1561,6 +1682,12 @@ void ResourceManager::make_controller_exported_state_interfaces_unavailable( void ResourceManager::remove_controller_exported_state_interfaces( const std::string & controller_name) { + if ( + resource_storage_->controllers_exported_state_interfaces_map_.find(controller_name) == + resource_storage_->controllers_exported_state_interfaces_map_.end()) + { + return; + } auto interface_names = resource_storage_->controllers_exported_state_interfaces_map_.at(controller_name); resource_storage_->controllers_exported_state_interfaces_map_.erase(controller_name); @@ -1623,6 +1750,12 @@ void ResourceManager::make_controller_reference_interfaces_unavailable( // CM API: Called in "callback/slow"-thread void ResourceManager::remove_controller_reference_interfaces(const std::string & controller_name) { + if ( + resource_storage_->controllers_reference_interfaces_map_.find(controller_name) == + resource_storage_->controllers_reference_interfaces_map_.end()) + { + return; + } auto interface_names = resource_storage_->controllers_reference_interfaces_map_.at(controller_name); resource_storage_->controllers_reference_interfaces_map_.erase(controller_name); @@ -1695,14 +1828,15 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin { if (!command_interface_is_available(key)) { - throw std::runtime_error(std::string("Command interface with '") + key + "' does not exist"); + throw std::runtime_error( + fmt::format(FMT_COMPILE("Command interface with key '{}' does not exist"), key)); } std::lock_guard guard_claimed(claimed_command_interfaces_lock_); if (command_interface_is_claimed(key)) { throw std::runtime_error( - std::string("Command interface with '") + key + "' is already claimed"); + fmt::format(FMT_COMPILE("Command interface with key '{}' is already claimed"), key)); } resource_storage_->claimed_command_interface_map_[key] = true; @@ -1748,31 +1882,46 @@ bool ResourceManager::command_interface_is_available(const std::string & name) c name) != resource_storage_->available_command_interfaces_.end(); } +std::string ResourceManager::get_command_interface_data_type(const std::string & name) const +{ + std::lock_guard guard(resource_interfaces_lock_); + auto it = resource_storage_->command_interface_map_.find(name); + if (it != resource_storage_->command_interface_map_.end()) + { + return it->second->get_data_type().to_string(); + } + else + { + throw std::runtime_error( + std::string("Command interface with '") + name + std::string("' does not exist")); + } +} + void ResourceManager::import_component( - std::unique_ptr actuator, const HardwareInfo & hardware_info) + std::unique_ptr actuator, const HardwareComponentParams & params) { std::lock_guard guard(resources_lock_); - resource_storage_->initialize_actuator(std::move(actuator), hardware_info); + resource_storage_->initialize_actuator(std::move(actuator), params); read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); } void ResourceManager::import_component( - std::unique_ptr sensor, const HardwareInfo & hardware_info) + std::unique_ptr sensor, const HardwareComponentParams & params) { std::lock_guard guard(resources_lock_); - resource_storage_->initialize_sensor(std::move(sensor), hardware_info); + resource_storage_->initialize_sensor(std::move(sensor), params); read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); } void ResourceManager::import_component( - std::unique_ptr system, const HardwareInfo & hardware_info) + std::unique_ptr system, const HardwareComponentParams & params) { std::lock_guard guard(resources_lock_); - resource_storage_->initialize_system(std::move(system), hardware_info); + resource_storage_->initialize_system(std::move(system), params); read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); @@ -1798,6 +1947,18 @@ ResourceManager::get_components_status() return resource_storage_->hardware_info_map_; } +const std::unordered_map & +ResourceManager::get_hard_joint_limits() const +{ + return resource_storage_->hard_joint_limits_; +} + +const std::unordered_map & +ResourceManager::get_soft_joint_limits() const +{ + return resource_storage_->soft_joint_limits_; +} + // CM API: Called in "callback/slow"-thread bool ResourceManager::prepare_command_mode_switch( const std::vector & start_interfaces, @@ -1861,12 +2022,38 @@ bool ResourceManager::prepare_command_mode_switch( return false; } + const auto & hardware_info_map = resource_storage_->hardware_info_map_; auto call_prepare_mode_switch = - [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) + [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger(), + allow_controller_activation_with_inactive_hardware = + allow_controller_activation_with_inactive_hardware_]( + auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer) { bool ret = true; for (auto & component : components) { + const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces; + find_common_hardware_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer); + find_common_hardware_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer); + if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty()) + { + RCLCPP_DEBUG( + logger, "Component '%s' after filtering has no command interfaces to switch", + component.get_name().c_str()); + continue; + } + if ( + !start_interfaces_buffer.empty() && + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && + !allow_controller_activation_with_inactive_hardware) + { + RCLCPP_WARN( + logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); + return false; + } if ( component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -1876,12 +2063,12 @@ bool ResourceManager::prepare_command_mode_switch( { if ( return_type::OK != - component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + component.prepare_command_mode_switch(start_interfaces_buffer, stop_interfaces_buffer)) { RCLCPP_ERROR( logger, "Component '%s' did not accept command interfaces combination: \n%s", component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } @@ -1892,7 +2079,8 @@ bool ResourceManager::prepare_command_mode_switch( "Exception of type : %s occurred while preparing command mode switch for component " "'%s' for the interfaces: \n %s : %s", typeid(e).name(), component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(), + e.what()); ret = false; } catch (...) @@ -1902,16 +2090,27 @@ bool ResourceManager::prepare_command_mode_switch( "Unknown exception occurred while preparing command mode switch for component '%s' for " "the interfaces: \n %s", component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } + else + { + RCLCPP_WARN( + logger, "Component '%s' is not in INACTIVE or ACTIVE state, skipping the prepare switch", + component.get_name().c_str()); + ret = false; + } } return ret; }; - const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_); - const bool systems_result = call_prepare_mode_switch(resource_storage_->systems_); + const bool actuators_result = call_prepare_mode_switch( + resource_storage_->actuators_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); + const bool systems_result = call_prepare_mode_switch( + resource_storage_->systems_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); return actuators_result && systems_result; } @@ -1927,12 +2126,38 @@ bool ResourceManager::perform_command_mode_switch( return true; } + const auto & hardware_info_map = resource_storage_->hardware_info_map_; auto call_perform_mode_switch = - [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) + [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger(), + allow_controller_activation_with_inactive_hardware = + allow_controller_activation_with_inactive_hardware_]( + auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer) { bool ret = true; for (auto & component : components) { + const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces; + find_common_hardware_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer); + find_common_hardware_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer); + if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty()) + { + RCLCPP_DEBUG( + logger, "Component '%s' after filtering has no command interfaces to perform switch", + component.get_name().c_str()); + continue; + } + if ( + !start_interfaces_buffer.empty() && + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && + !allow_controller_activation_with_inactive_hardware) + { + RCLCPP_WARN( + logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); + return false; + } if ( component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -1942,10 +2167,12 @@ bool ResourceManager::perform_command_mode_switch( { if ( return_type::OK != - component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + component.perform_command_mode_switch(start_interfaces_buffer, stop_interfaces_buffer)) { RCLCPP_ERROR( - logger, "Component '%s' could not perform switch", component.get_name().c_str()); + logger, "Component '%s' could not perform switch for the command interfaces: \n%s", + component.get_name().c_str(), + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } @@ -1956,7 +2183,8 @@ bool ResourceManager::perform_command_mode_switch( "Exception of type : %s occurred while performing command mode switch for component " "'%s' for the interfaces: \n %s : %s", typeid(e).name(), component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str(), + e.what()); ret = false; } catch (...) @@ -1967,16 +2195,27 @@ bool ResourceManager::perform_command_mode_switch( "for " "the interfaces: \n %s", component.get_name().c_str(), - interfaces_to_string(start_interfaces, stop_interfaces).c_str()); + interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str()); ret = false; } } + else + { + RCLCPP_WARN( + logger, "Component '%s' is not in INACTIVE or ACTIVE state, skipping the perform switch", + component.get_name().c_str()); + ret = false; + } } return ret; }; - const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_); - const bool systems_result = call_perform_mode_switch(resource_storage_->systems_); + const bool actuators_result = call_perform_mode_switch( + resource_storage_->actuators_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); + const bool systems_result = call_perform_mode_switch( + resource_storage_->systems_, resource_storage_->start_interfaces_buffer_, + resource_storage_->stop_interfaces_buffer_); if (actuators_result && systems_result) { @@ -2061,6 +2300,7 @@ return_type ResourceManager::set_component_state( }; std::lock_guard guard(resources_lock_); + std::lock_guard limiters_guard(joint_limiters_lock_); bool found = find_set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->actuators_); @@ -2103,9 +2343,9 @@ bool ResourceManager::enforce_command_limits(const rclcpp::Duration & period) // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - read_write_status.ok = true; + read_write_status.result = return_type::OK; read_write_status.failed_hardware_names.clear(); // This is needed while we load and initialize the components @@ -2180,26 +2420,16 @@ HardwareReadWriteStatus ResourceManager::read( component_name.c_str()); ret_val = return_type::ERROR; } - if (ret_val == return_type::ERROR) + RCLCPP_WARN_EXPRESSION( + get_logger(), ret_val == hardware_interface::return_type::DEACTIVATE, + "DEACTIVATE returned from read cycle is treated the same as ERROR."); + if (ret_val != return_type::OK) { component.error(); - read_write_status.ok = false; + read_write_status.result = return_type::ERROR; read_write_status.failed_hardware_names.push_back(component_name); resource_storage_->remove_all_hardware_interfaces_from_available_list(component_name); } - else if (ret_val == return_type::DEACTIVATE) - { - rclcpp_lifecycle::State inactive_state( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); - set_component_state(component_name, inactive_state); - } - // If desired: automatic re-activation. We could add a flag for this... - // else - // { - // using lifecycle_msgs::msg::State; - // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); - // set_component_state(component.get_name(), state); - // } } }; @@ -2212,9 +2442,9 @@ HardwareReadWriteStatus ResourceManager::read( // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::write( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - read_write_status.ok = true; + read_write_status.result = return_type::OK; read_write_status.failed_hardware_names.clear(); // This is needed while we load and initialize the components @@ -2293,7 +2523,7 @@ HardwareReadWriteStatus ResourceManager::write( if (ret_val == return_type::ERROR) { component.error(); - read_write_status.ok = false; + read_write_status.result = ret_val; read_write_status.failed_hardware_names.push_back(component_name); resource_storage_->remove_all_hardware_interfaces_from_available_list(component_name); } @@ -2302,6 +2532,11 @@ HardwareReadWriteStatus ResourceManager::write( rclcpp_lifecycle::State inactive_state( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); set_component_state(component_name, inactive_state); + read_write_status.result = ret_val; + if (return_failed_hardware_names_on_return_deactivate_write_cycle_) + { + read_write_status.failed_hardware_names.push_back(component_name); + } } } }; @@ -2363,6 +2598,20 @@ rclcpp::Clock::SharedPtr ResourceManager::get_clock() const // BEGIN: private methods +hardware_interface::ResourceManagerParams ResourceManager::constructParams( + rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, const std::string & urdf, + bool activate_all, unsigned int update_rate) +{ + hardware_interface::ResourceManagerParams params; + params.clock = clock; + params.logger = logger; + params.robot_description = urdf; + params.activate_all = activate_all; + params.update_rate = update_rate; + + return params; +} + bool ResourceManager::validate_storage( const std::vector & hardware_info) const { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp deleted file mode 100644 index 7d3a318117..0000000000 --- a/hardware_interface/src/sensor.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 - 2021 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "hardware_interface/sensor.hpp" - -#include -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/lifecycle_helpers.hpp" -#include "hardware_interface/sensor_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace hardware_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} - -Sensor::Sensor(Sensor && other) noexcept -{ - std::lock_guard lock(other.sensors_mutex_); - impl_ = std::move(other.impl_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); -} - -const rclcpp_lifecycle::State & Sensor::initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -{ - return this->initialize(sensor_info, logger, clock_interface->get_clock()); -} - -const rclcpp_lifecycle::State & Sensor::initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -{ - std::unique_lock lock(sensors_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) - { - switch (impl_->init(sensor_info, logger, clock)) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::configure() -{ - std::unique_lock lock(sensors_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_configure(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::cleanup() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_cleanup(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::shutdown() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) - { - switch (impl_->on_shutdown(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::activate() -{ - std::unique_lock lock(sensors_mutex_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - read_statistics_.reset_statistics(); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_activate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->enable_introspection(true); - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::deactivate() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - switch (impl_->on_deactivate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::error() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_error(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -std::vector Sensor::export_state_interfaces() -{ - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interfaces = impl_->export_state_interfaces(); - // END: for backward compatibility - - // If no StateInterfaces has been exported, this could mean: - // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well - // b) default implementation for export_state_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_state_interfaces(); - } - - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto const & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(interface)); - } - return interface_ptrs; - // END: for backward compatibility -} - -const std::string & Sensor::get_name() const { return impl_->get_name(); } - -const std::string & Sensor::get_group_name() const { return impl_->get_group_name(); } - -const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const -{ - return impl_->get_lifecycle_state(); -} - -const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } - -const HardwareComponentStatisticsCollector & Sensor::get_read_statistics() const -{ - return read_statistics_; -} - -return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_read_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_read(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - read_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_read_cycle_time_).seconds()); - } - last_read_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -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 deleted file mode 100644 index fc401bb0cc..0000000000 --- a/hardware_interface/src/system.cpp +++ /dev/null @@ -1,398 +0,0 @@ -// Copyright 2020 - 2021 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "hardware_interface/system.hpp" - -#include -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/lifecycle_helpers.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace hardware_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} - -System::System(System && other) noexcept -{ - std::lock_guard lock(other.system_mutex_); - impl_ = std::move(other.impl_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); -} - -const rclcpp_lifecycle::State & System::initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -{ - return this->initialize(system_info, logger, clock_interface->get_clock()); -} - -const rclcpp_lifecycle::State & System::initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -{ - std::unique_lock lock(system_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) - { - switch (impl_->init(system_info, logger, clock)) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::configure() -{ - std::unique_lock lock(system_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_configure(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::cleanup() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_cleanup(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::shutdown() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) - { - switch (impl_->on_shutdown(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::activate() -{ - std::unique_lock lock(system_mutex_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - read_statistics_.reset_statistics(); - write_statistics_.reset_statistics(); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - impl_->prepare_for_activation(); - switch (impl_->on_activate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->enable_introspection(true); - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::deactivate() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - switch (impl_->on_deactivate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::error() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_error(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -std::vector System::export_state_interfaces() -{ - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interfaces = impl_->export_state_interfaces(); - // END: for backward compatibility - - // If no StateInterfaces has been exported, this could mean: - // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well - // b) default implementation for export_state_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_state_interfaces(); - } - - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto const & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(interface)); - } - return interface_ptrs; - // END: for backward compatibility -} - -std::vector System::export_command_interfaces() -{ - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interfaces = impl_->export_command_interfaces(); - // END: for backward compatibility - - // If no CommandInterface has been exported, this could mean: - // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well - // b) default implementation for export_command_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_command_interfaces(); - } - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(std::move(interface))); - } - return interface_ptrs; - // END: for backward compatibility -} - -return_type System::prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - return impl_->prepare_command_mode_switch(start_interfaces, stop_interfaces); -} - -return_type System::perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); -} - -const std::string & System::get_name() const { return impl_->get_name(); } - -const std::string & System::get_group_name() const { return impl_->get_group_name(); } - -const rclcpp_lifecycle::State & System::get_lifecycle_state() const -{ - return impl_->get_lifecycle_state(); -} - -const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle_time_; } - -const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } - -const HardwareComponentStatisticsCollector & System::get_read_statistics() const -{ - return read_statistics_; -} - -const HardwareComponentStatisticsCollector & System::get_write_statistics() const -{ - return write_statistics_; -} - -return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_read_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_read(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - read_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_read_cycle_time_).seconds()); - } - last_read_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_write_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_write(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - write_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - write_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_write_cycle_time_).seconds()); - } - last_write_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -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 38bf2fa5e5..7333c3d12d 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -20,8 +20,11 @@ #include #include "gmock/gmock.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#pragma GCC diagnostic pop #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -32,7 +35,8 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto PERIOD_SEC = 0.1; // 0.1 seconds for easier math +const auto PERIOD = rclcpp::Duration::from_seconds(PERIOD_SEC); // 0.1 seconds for easier math const auto COMPARE_DELTA = 0.0001; } // namespace @@ -43,7 +47,7 @@ class TestGenericSystem : public ::testing::Test void test_generic_system_with_mock_sensor_commands( std::string & urdf, const std::string & component_name); void test_generic_system_with_mock_gpio_commands( - std::string & urdf, const std::string & component_name); + std::string & urdf, const std::string & component_name, const bool data_type_bool = false); protected: void SetUp() override @@ -103,7 +107,9 @@ class TestGenericSystem : public ::testing::Test 3.45 - + + 0.0 + @@ -111,12 +117,14 @@ class TestGenericSystem : public ::testing::Test 2.78 - + + 0.0 + )"; - hardware_system_2dof_with_other_interface_ = + hardware_system_2dof_with_gpio_ = R"( @@ -272,7 +280,9 @@ class TestGenericSystem : public ::testing::Test 3.45 - + + 0.0 + @@ -280,7 +290,9 @@ class TestGenericSystem : public ::testing::Test 2.78 - + + 0.0 + )"; @@ -331,7 +343,9 @@ class TestGenericSystem : public ::testing::Test 3.45 - + + 0.0 + @@ -363,7 +377,9 @@ class TestGenericSystem : public ::testing::Test 3.45 - + + 0.0 + @@ -371,7 +387,9 @@ class TestGenericSystem : public ::testing::Test 2.78 - + + 0.0 + @@ -399,7 +417,9 @@ class TestGenericSystem : public ::testing::Test 3.45 - + + 0.0 + @@ -407,7 +427,9 @@ class TestGenericSystem : public ::testing::Test 2.78 - + + 0.0 + @@ -422,6 +444,46 @@ class TestGenericSystem : public ::testing::Test )"; + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_ = + R"( + + + mock_components/GenericSystem + true + + + + + + 3.45 + + + 0.0 + + + + + + + 2.78 + + + 0.0 + + + + + + + + + + + + + +)"; + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( @@ -435,7 +497,9 @@ class TestGenericSystem : public ::testing::Test 3.45 - + + 0.0 + @@ -443,7 +507,9 @@ class TestGenericSystem : public ::testing::Test 2.78 - + + 0.0 + @@ -492,6 +558,8 @@ class TestGenericSystem : public ::testing::Test )"; + // joint 1: position & velocity & effort control + // joint 2: velocity & acceleration control hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( @@ -502,6 +570,7 @@ class TestGenericSystem : public ::testing::Test + 3.45 @@ -524,7 +593,97 @@ class TestGenericSystem : public ::testing::Test )"; - valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + hardware_system_2dof_with_position_control_mode_position_state_only_ = + R"( + + + mock_components/GenericSystem + true + + + + + 3.45 + + + + + + 2.78 + + + +)"; + + hardware_system_2dof_with_position_control_mode_position_state_only_w_offset_ = + R"( + + + mock_components/GenericSystem + -3 + true + + + + + 3.45 + + + + + + 2.78 + + + +)"; + + hardware_system_2dof_with_velocity_control_mode_position_state_only_ = + R"( + + + mock_components/GenericSystem + true + + + + + 3.45 + + + + + + 2.78 + + + +)"; + + hardware_system_2dof_with_velocity_control_mode_position_state_only_w_offset_ = + R"( + + + mock_components/GenericSystem + -3 + true + + + + + 3.45 + + + + + + 2.78 + + + +)"; + + hardware_system_3dof_standard_interfaces_with_different_control_modes_ = R"( @@ -643,12 +802,18 @@ class TestGenericSystem : public ::testing::Test )"; + + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY( + node_, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, + hardware_interface::DEFAULT_REGISTRY_KEY); } + void TearDown() override { node_.reset(); } + std::string hardware_system_2dof_; std::string hardware_system_2dof_asymetric_; std::string hardware_system_2dof_standard_interfaces_; - std::string hardware_system_2dof_with_other_interface_; + std::string hardware_system_2dof_with_gpio_; std::string hardware_system_2dof_with_sensor_; std::string hardware_system_2dof_with_sensor_mock_command_; std::string hardware_system_2dof_with_sensor_mock_command_True_; @@ -658,15 +823,21 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_; std::string valid_urdf_ros2_control_system_robot_with_gpio_; std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_; std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; - std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_; + std::string hardware_system_2dof_with_position_control_mode_position_state_only_; + std::string hardware_system_2dof_with_position_control_mode_position_state_only_w_offset_; + std::string hardware_system_2dof_standard_interfaces_with_velocity_control_mode_; + std::string hardware_system_2dof_with_velocity_control_mode_position_state_only_; + std::string hardware_system_2dof_with_velocity_control_mode_position_state_only_w_offset_; + std::string hardware_system_3dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; std::string hardware_system_2dof_standard_interfaces_with_same_hardware_group_; std::string hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_; - rclcpp::Node node_ = rclcpp::Node("TestGenericSystem"); + rclcpp::Node::SharedPtr node_ = std::make_shared("TestGenericSystem"); }; // Forward declaration @@ -678,30 +849,17 @@ class ResourceStorage; class TestableResourceManager : public hardware_interface::ResourceManager { public: - friend TestGenericSystem; - - FRIEND_TEST(TestGenericSystem, generic_system_2dof_symetric_interfaces); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True); - FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True); - - explicit TestableResourceManager(rclcpp::Node & node) + explicit TestableResourceManager(rclcpp::Node::SharedPtr node) : hardware_interface::ResourceManager( - node.get_node_clock_interface(), node.get_node_logging_interface()) + node->get_node_clock_interface(), node->get_node_logging_interface()) { } explicit TestableResourceManager( - rclcpp::Node & node, const std::string & urdf, bool activate_all = false, + rclcpp::Node::SharedPtr 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, + urdf, node->get_node_clock_interface(), node->get_node_logging_interface(), activate_all, cm_update_rate) { } @@ -836,22 +994,22 @@ void generic_system_functional_test( const std::string & urdf, const std::string component_name = "GenericSystem2dof", const double offset = 0) { - rclcpp::Node node("test_generic_system"); + rclcpp::Node::SharedPtr node = std::make_shared("test_generic_system"); TestableResourceManager rm(node, urdf); // check is hardware is configured auto status_map = rm.get_components_status(); - EXPECT_EQ( + ASSERT_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( + ASSERT_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( + ASSERT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); EXPECT_EQ(status_map[component_name].rw_rate, 100u); @@ -865,15 +1023,14 @@ void generic_system_functional_test( hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); - // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); - ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); - ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); - ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j1v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2v_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -882,36 +1039,36 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values - ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands + offset to states - ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -920,14 +1077,14 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.55, j1p_c.get_optional().value()); - ASSERT_EQ(0.66, j1v_c.get_optional().value()); - ASSERT_EQ(0.77, j2p_c.get_optional().value()); - ASSERT_EQ(0.88, j2v_c.get_optional().value()); + EXPECT_EQ(0.11 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.55, j1p_c.get_optional().value()); + EXPECT_EQ(0.66, j1v_c.get_optional().value()); + EXPECT_EQ(0.77, j2p_c.get_optional().value()); + EXPECT_EQ(0.88, j2v_c.get_optional().value()); deactivate_components(rm, {component_name}); status_map = rm.get_components_status(); @@ -938,7 +1095,7 @@ void generic_system_functional_test( 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"); + rclcpp::Node::SharedPtr node = std::make_shared("test_generic_system"); TestableResourceManager rm(node, urdf, false, 200u); const std::string component1 = component_prefix + "1"; const std::string component2 = component_prefix + "2"; @@ -946,17 +1103,17 @@ void generic_system_error_group_test( auto status_map = rm.get_components_status(); for (auto component : {component1, component2}) { - EXPECT_EQ( + ASSERT_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( + ASSERT_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( + ASSERT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); EXPECT_EQ(status_map[component].rw_rate, 200u); } @@ -971,11 +1128,11 @@ void generic_system_error_group_test( hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); - // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + // State interfaces without initial value are set to nan + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1v_s.get_optional().value())); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j2v_s.get_optional().value())); ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); @@ -988,36 +1145,36 @@ void generic_system_error_group_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1v_s.get_optional().value())); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j2v_s.get_optional().value())); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values - ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1v_s.get_optional().value())); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j2v_s.get_optional().value())); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands to states - ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - ASSERT_EQ(0.11, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -1026,21 +1183,21 @@ void generic_system_error_group_test( ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - ASSERT_EQ(0.11, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.55, j1p_c.get_optional().value()); - ASSERT_EQ(0.66, j1v_c.get_optional().value()); - ASSERT_EQ(0.77, j2p_c.get_optional().value()); - ASSERT_EQ(0.88, j2v_c.get_optional().value()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.55, j1p_c.get_optional().value()); + EXPECT_EQ(0.66, j1v_c.get_optional().value()); + EXPECT_EQ(0.77, j2p_c.get_optional().value()); + EXPECT_EQ(0.88, j2v_c.get_optional().value()); // Error testing ASSERT_TRUE(j1p_c.set_value(std::numeric_limits::infinity())); ASSERT_TRUE(j1v_c.set_value(std::numeric_limits::infinity())); // read() should now bring error in the first component auto read_result = rm.read(TIME, PERIOD); - ASSERT_FALSE(read_result.ok); + EXPECT_EQ(read_result.result, hardware_interface::return_type::ERROR); if (validate_same_group) { // If they belong to the same group, show the error in all hardware components of same group @@ -1076,12 +1233,12 @@ void generic_system_error_group_test( // Error should be recoverable only after reactivating the hardware component ASSERT_TRUE(j1p_c.set_value(0.0)); ASSERT_TRUE(j1v_c.set_value(0.0)); - ASSERT_FALSE(rm.read(TIME, PERIOD).ok); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::ERROR); // Now it should be recoverable deactivate_components(rm, {component1}); activate_components(rm, {component1}); - ASSERT_TRUE(rm.read(TIME, PERIOD).ok); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); deactivate_components(rm, {component1, component2}); status_map = rm.get_components_status(); @@ -1119,7 +1276,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_same_group) TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ + + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_gpio_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available @@ -1178,7 +1335,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ASSERT_EQ(0.99, vo_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(1.55, j1p_s.get_optional().value()); ASSERT_EQ(0.1, j1v_s.get_optional().value()); ASSERT_EQ(0.65, j2p_s.get_optional().value()); @@ -1189,7 +1346,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ASSERT_EQ(0.99, vo_c.get_optional().value()); // read() mirrors commands to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.11, j1p_s.get_optional().value()); ASSERT_EQ(0.1, j1v_s.get_optional().value()); ASSERT_EQ(0.33, j2p_s.get_optional().value()); @@ -1246,10 +1403,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) EXPECT_ANY_THROW(rm.claim_command_interface("tcp_force_sensor/tx")); EXPECT_ANY_THROW(rm.claim_command_interface("tcp_force_sensor/ty")); - ASSERT_EQ(0.0, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + // State interfaces without initial value are set to nan + ASSERT_TRUE(std::isnan(j1p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); @@ -1262,10 +1420,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_TRUE(j2p_c.set_value(0.33)); // State values should not be changed - ASSERT_EQ(0.0, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); @@ -1274,11 +1432,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_EQ(0.33, j2p_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); - ASSERT_EQ(0.0, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + ASSERT_TRUE(std::isnan(j1p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); @@ -1287,15 +1445,15 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_EQ(0.33, j2p_c.get_optional().value()); // read() mirrors commands to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.11, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); ASSERT_EQ(0.33, j2p_s.get_optional().value()); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_EQ(0.33, j2p_c.get_optional().value()); } @@ -1349,10 +1507,11 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( hardware_interface::LoanedCommandInterface sty_c = rm.claim_command_interface("tcp_force_sensor/ty"); - ASSERT_EQ(0.0, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + // State interfaces without initial value are set to nan + ASSERT_TRUE(std::isnan(j1p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); @@ -1373,10 +1532,10 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( ASSERT_TRUE(sty_c.set_value(4.44)); // State values should not be changed - ASSERT_EQ(0.0, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); @@ -1389,11 +1548,11 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( ASSERT_EQ(4.44, sty_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); - ASSERT_EQ(0.0, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + ASSERT_TRUE(std::isnan(j1p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); @@ -1406,11 +1565,11 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( ASSERT_EQ(4.44, sty_c.get_optional().value()); // read() mirrors commands to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.11, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); ASSERT_EQ(0.33, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); ASSERT_EQ(1.11, sfx_s.get_optional().value()); ASSERT_EQ(2.22, sfy_s.get_optional().value()); ASSERT_EQ(3.33, stx_s.get_optional().value()); @@ -1467,10 +1626,11 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + // State interfaces without initial value are set to nan ASSERT_EQ(1.57, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); @@ -1480,23 +1640,23 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( // State values should not be changed ASSERT_EQ(1.57, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_EQ(0.05, j1v_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(1.57, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(0.0, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_EQ(0.05, j1v_c.get_optional().value()); // read() mirrors commands to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.11, j1p_s.get_optional().value()); ASSERT_EQ(0.05, j1v_s.get_optional().value()); ASSERT_EQ(-0.22, j2p_s.get_optional().value()); @@ -1575,7 +1735,6 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i hardware_interface::LoanedStateInterface c_j2p_s = rm.claim_state_interface("joint2/actual_position"); - // State interfaces without initial value are set to 0 ASSERT_EQ(3.45, j1p_s.get_optional().value()); ASSERT_EQ(0.0, j1v_s.get_optional().value()); ASSERT_EQ(2.78, j2p_s.get_optional().value()); @@ -1602,7 +1761,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(3.45, j1p_s.get_optional().value()); ASSERT_EQ(0.0, j1v_s.get_optional().value()); ASSERT_EQ(2.78, j2p_s.get_optional().value()); @@ -1613,7 +1772,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands + offset to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.11, j1p_s.get_optional().value()); ASSERT_EQ(0.11 + offset, c_j1p_s.get_optional().value()); ASSERT_EQ(0.22, j1v_s.get_optional().value()); @@ -1703,7 +1862,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) hardware_interface::LoanedCommandInterface gpio2_vac_c = rm.claim_command_interface("flange_vacuum/vacuum"); - // State interfaces without initial value are set to 0 + // State interfaces without initial value are set to nan ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); ASSERT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); ASSERT_TRUE(std::isnan(gpio1_a_o1_c.get_optional().value())); @@ -1720,14 +1879,14 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_EQ(0.222, gpio2_vac_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); ASSERT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); ASSERT_EQ(0.111, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.222, gpio2_vac_c.get_optional().value()); // read() mirrors commands + offset to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.111, gpio1_a_o1_s.get_optional().value()); ASSERT_EQ(0.222, gpio2_vac_s.get_optional().value()); ASSERT_EQ(0.111, gpio1_a_o1_c.get_optional().value()); @@ -1748,7 +1907,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) } void TestGenericSystem::test_generic_system_with_mock_gpio_commands( - std::string & urdf, const std::string & component_name) + std::string & urdf, const std::string & component_name, const bool data_type_bool) { TestableResourceManager rm(node_, urdf); @@ -1809,49 +1968,90 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + if (data_type_bool) + { + // for bool data type initial value is false + EXPECT_FALSE(gpio2_vac_s.get_optional().value()); + EXPECT_FALSE(gpio2_vac_c.get_optional().value()); + } + else + { + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value())); + } EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(gpio1_a_o1_c.set_value(0.11)); ASSERT_TRUE(gpio1_a_i1_c.set_value(0.33)); ASSERT_TRUE(gpio1_a_i2_c.set_value(1.11)); - ASSERT_TRUE(gpio2_vac_c.set_value(2.22)); + if (data_type_bool) + { + ASSERT_TRUE(gpio2_vac_c.set_value(true)); + } + else + { + ASSERT_TRUE(gpio2_vac_c.set_value(2.22)); + } // State values should not be changed EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + if (data_type_bool) + { + EXPECT_FALSE(gpio2_vac_s.get_optional().value()); + EXPECT_TRUE(gpio2_vac_c.get_optional().value()); + } + else + { + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); + } ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + if (data_type_bool) + { + // for bool data type initial value is false + EXPECT_FALSE(gpio2_vac_s.get_optional().value()); + EXPECT_TRUE(gpio2_vac_c.get_optional().value()); + } + else + { + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); + } ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); // read() mirrors commands to states - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(0.11, gpio1_a_o1_s.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_s.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_o2_s.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value()); + if (data_type_bool) + { + EXPECT_TRUE(gpio2_vac_s.get_optional().value()); + EXPECT_TRUE(gpio2_vac_c.get_optional().value()); + } + else + { + ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); + } ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) @@ -1863,6 +2063,15 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); } +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem", true); +} + TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) { auto urdf = ros2_control_test_assets::urdf_head + @@ -1940,9 +2149,10 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); - ASSERT_EQ(5u, rm.command_interface_keys().size()); + ASSERT_EQ(6u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint1/effort")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); @@ -1956,26 +2166,34 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j1e_c = rm.claim_command_interface("joint1/effort"); hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); - EXPECT_EQ(3.45, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); - EXPECT_EQ(0.0, j2v_s.get_optional().value()); - EXPECT_EQ(0.0, j2a_s.get_optional().value()); + // State interfaces without initial value are set to nan + double j1p = 3.45; + double j2p = 2.78; + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1a_s.get_optional().value())); + EXPECT_EQ(j2p, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j2v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2a_s.get_optional().value())); ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j1e_c.get_optional().value())); // not used in this test ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2a_c.get_optional().value())); // Test error management in prepare mode switch - ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface - rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); - ASSERT_EQ( // joint1 has two interfaces - rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); + ASSERT_EQ( // joint1 interface does not exist + rm.prepare_command_mode_switch({"joint1/unknown", "joint2/acceleration"}, {}), false); + ASSERT_EQ( // joint1 has non 'position', 'velocity', or 'acceleration' interface + rm.prepare_command_mode_switch({"joint1/effort", "joint2/acceleration"}, {}), false); + ASSERT_EQ( // joint2 has two interfaces + rm.prepare_command_mode_switch({"joint1/position", "joint2/velocity", "joint2/acceleration"}, + {}), false); // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored ASSERT_EQ( @@ -1992,64 +2210,68 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) ASSERT_TRUE(j2a_c.set_value(3.5)); // State values should not be changed - EXPECT_EQ(3.45, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); - EXPECT_EQ(0.0, j2v_s.get_optional().value()); - EXPECT_EQ(0.0, j2a_s.get_optional().value()); + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1a_s.get_optional().value())); + EXPECT_EQ(j2p, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j2v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2a_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); ASSERT_EQ(3.5, j2a_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); - EXPECT_EQ(3.45, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); - EXPECT_EQ(0.0, j2v_s.get_optional().value()); - EXPECT_EQ(0.0, j2a_s.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1a_s.get_optional().value())); + EXPECT_EQ(j2p, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j2v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2a_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_optional().value()); - EXPECT_EQ(-33.4, j1v_s.get_optional().value()); - EXPECT_NEAR(-334.0, j1a_s.get_optional().value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); - EXPECT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(j1p = 0.11, j1p_s.get_optional().value()); + double j1v = (j1p - 3.45) / PERIOD_SEC; + EXPECT_NEAR(j1v, j1v_s.get_optional().value(), COMPARE_DELTA); + double j1a = (j1v - 0.0) / PERIOD_SEC; + EXPECT_NEAR(j1a, j1a_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(3.5, j2a_s.get_optional().value()); + double j2v = 0.0 + 3.5 * PERIOD_SEC; + EXPECT_NEAR(j2v, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(j2p += j2v * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_NEAR(334.0, j1a_s.get_optional().value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); - EXPECT_NEAR(0.35, j2v_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + const double j1v_old = j1v; + EXPECT_EQ(j1v = 0.0, j1v_s.get_optional().value()); + EXPECT_NEAR(j1a = (j1v - j1v_old) / PERIOD_SEC, j1a_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(3.5, j2a_s.get_optional().value()); + EXPECT_NEAR(j2v += 3.5 * PERIOD_SEC, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(j2p += j2v * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.815, j2p_s.get_optional().value()); - EXPECT_NEAR(0.7, j2v_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + EXPECT_EQ(j1v, j1v_s.get_optional().value()); + EXPECT_EQ(j1a = 0.0, j1a_s.get_optional().value()); EXPECT_EQ(3.5, j2a_s.get_optional().value()); + EXPECT_NEAR(j2v += 3.5 * PERIOD_SEC, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(j2p += j2v * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); ASSERT_EQ(0.11, j1p_c.get_optional().value()); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); @@ -2064,55 +2286,357 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) ASSERT_TRUE(j2v_c.set_value(2.0)); // State values should not be changed - EXPECT_EQ(0.11, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.815, j2p_s.get_optional().value()); - EXPECT_NEAR(0.7, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + EXPECT_EQ(j1v, j1v_s.get_optional().value()); + EXPECT_EQ(j1a, j1a_s.get_optional().value()); + EXPECT_NEAR(j2p, j2p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(j2v, j2v_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(3.5, j2a_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); // is not cleared, but not used anymore ASSERT_EQ(0.5, j1v_c.get_optional().value()); ASSERT_EQ(2.0, j2v_c.get_optional().value()); - ASSERT_EQ(3.5, j2a_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // is not cleared, but not used anymore // write() does not change values - rm.write(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_optional().value()); - EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.815, j2p_s.get_optional().value()); - EXPECT_NEAR(0.7, j2v_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(j1p, j1p_s.get_optional().value()); + EXPECT_EQ(j1v, j1v_s.get_optional().value()); + EXPECT_EQ(j1a, j1a_s.get_optional().value()); + EXPECT_NEAR(j2p, j2p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(j2v, j2v_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(3.5, j2a_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); // is not cleared, but not used anymore ASSERT_EQ(0.5, j1v_c.get_optional().value()); ASSERT_EQ(2.0, j2v_c.get_optional().value()); - ASSERT_EQ(3.5, j2a_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // is not cleared, but not used anymore // read() mirrors commands to states and calculate dynamics (both velocity mode) - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(j1p += 0.5 * PERIOD_SEC, j1p_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(0.5, j1v_s.get_optional().value()); - EXPECT_EQ(5.0, j1a_s.get_optional().value()); - EXPECT_EQ(2.885, j2p_s.get_optional().value()); + EXPECT_EQ((0.5 - j1v) / PERIOD_SEC, j1a_s.get_optional().value()); + EXPECT_NEAR(j2p += 2.0 * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(2.0, j2v_s.get_optional().value()); - EXPECT_NEAR(13.0, j2a_s.get_optional().value(), COMPARE_DELTA); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_NEAR((2.0 - j2v) / PERIOD_SEC, j2a_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); // is not cleared, but not used anymore ASSERT_EQ(0.5, j1v_c.get_optional().value()); ASSERT_EQ(2.0, j2v_c.get_optional().value()); - ASSERT_EQ(3.5, j2a_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // is not cleared, but not used anymore // read() mirrors commands to states and calculate dynamics (both velocity mode) - rm.read(TIME, PERIOD); - EXPECT_EQ(0.16, j1p_s.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(j1p += 0.5 * PERIOD_SEC, j1p_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(0.5, j1v_s.get_optional().value()); EXPECT_EQ(0.0, j1a_s.get_optional().value()); - EXPECT_EQ(3.085, j2p_s.get_optional().value()); + EXPECT_NEAR(j2p += 2.0 * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); EXPECT_EQ(2.0, j2v_s.get_optional().value()); EXPECT_EQ(0.0, j2a_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); // is not cleared, but not used anymore ASSERT_EQ(0.5, j1v_c.get_optional().value()); ASSERT_EQ(2.0, j2v_c.get_optional().value()); - ASSERT_EQ(3.5, j2a_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // is not cleared, but not used anymore +} + +TEST_F(TestGenericSystem, simple_dynamics_pos_control_modes_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_with_position_control_mode_position_state_only_ + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint2/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); + + // State interfaces without initial value are set to nan + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/position", "joint2/position"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/position", "joint2/position"}, {}), true); + + // set some new values in commands + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j2p_c.set_value(3.5)); + + // State values should not be changed + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // write() does not change values + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // read() mirrors commands to states, no dynamics to calculate actually + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(3.5, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // read() mirrors commands to states again, no dynamics to calculate actually + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(3.5, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // read() mirrors commands to states again, no dynamics to calculate actually + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(3.5, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); +} + +TEST_F(TestGenericSystem, simple_dynamics_pos_control_modes_interfaces_w_offset) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_with_position_control_mode_position_state_only_w_offset_ + + ros2_control_test_assets::urdf_tail; + constexpr double offset = -3.0; + + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint2/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); + + // State interfaces without initial value are set to nan + EXPECT_NEAR(3.45 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/position", "joint2/position"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/position", "joint2/position"}, {}), true); + + // set some new values in commands + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j2p_c.set_value(3.5)); + + // State values should not be changed + EXPECT_NEAR(3.45 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // write() does not change values + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // read() mirrors commands to states, no dynamics to calculate actually + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(0.11 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(3.5 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // read() mirrors commands to states again, no dynamics to calculate actually + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(0.11 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(3.5 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); + + // read() mirrors commands to states again, no dynamics to calculate actually + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(0.11 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(3.5 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(3.5, j2p_c.get_optional().value()); +} + +TEST_F(TestGenericSystem, simple_dynamics_vel_control_modes_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_with_velocity_control_mode_position_state_only_ + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + + // State interfaces without initial value are set to nan + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + ASSERT_TRUE(j1v_c.set_value(0.11)); + ASSERT_TRUE(j2v_c.set_value(3.5)); + + // State values should not be changed + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // write() does not change values + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // read() mirrors commands to states and integrates positions + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + 0.11 * PERIOD_SEC, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + 3.5 * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // read() mirrors commands to states again and integrates positions + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + 0.11 * 2 * PERIOD_SEC, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + 3.5 * 2 * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // read() mirrors commands to states again and integrates positions + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + 0.11 * 3 * PERIOD_SEC, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + 3.5 * 3 * PERIOD_SEC, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); +} + +TEST_F(TestGenericSystem, simple_dynamics_vel_control_modes_interfaces_with_offset) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_with_velocity_control_mode_position_state_only_w_offset_ + + ros2_control_test_assets::urdf_tail; + constexpr double offset = -3.0; + + TestableResourceManager rm(node_, urdf); + // Activate components to get all interfaces available + activate_components(rm, {"MockHardwareSystem"}); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + + // State interfaces without initial value are set to nan + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + ASSERT_TRUE(j1v_c.set_value(0.11)); + ASSERT_TRUE(j2v_c.set_value(3.5)); + + // State values should not be changed + EXPECT_NEAR(3.45 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // write() does not change values + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // read() mirrors commands to states and integrates positions + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + 0.11 * PERIOD_SEC + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + 3.5 * PERIOD_SEC + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // read() mirrors commands to states again and integrates positions + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + 0.11 * 2 * PERIOD_SEC + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + 3.5 * 2 * PERIOD_SEC + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); + + // read() mirrors commands to states again and integrates positions + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_NEAR(3.45 + 0.11 * 3 * PERIOD_SEC + offset, j1p_s.get_optional().value(), COMPARE_DELTA); + EXPECT_NEAR(2.78 + 3.5 * 3 * PERIOD_SEC + offset, j2p_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1v_c.get_optional().value()); + ASSERT_EQ(3.5, j2v_c.get_optional().value()); } TEST_F(TestGenericSystem, disabled_commands_flag_is_active) @@ -2138,7 +2662,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); // set some new values in commands @@ -2146,19 +2670,19 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); // write() does not change values - rm.write(TIME, PERIOD); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); // read() also does not change values - rm.read(TIME, PERIOD); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_s.get_optional().value())); ASSERT_EQ(0.11, j1p_c.get_optional().value()); } @@ -2179,7 +2703,7 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_)); ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_asymetric_)); ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_)); - ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_other_interface_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_gpio_)); ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_)); ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); ASSERT_TRUE( @@ -2195,17 +2719,74 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_)); ASSERT_TRUE(check_prepare_command_mode_switch( valid_urdf_ros2_control_system_robot_with_gpio_mock_command_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_)); ASSERT_TRUE(check_prepare_command_mode_switch( valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_)); ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_)); ASSERT_TRUE(check_prepare_command_mode_switch(gpio_with_initial_value_)); + ASSERT_FALSE(check_prepare_command_mode_switch( hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_with_position_control_mode_position_state_only_)); ASSERT_TRUE(check_prepare_command_mode_switch( - valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + hardware_system_2dof_with_velocity_control_mode_position_state_only_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_3dof_standard_interfaces_with_different_control_modes_)); ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_)); } +TEST_F(TestGenericSystem, perform_command_mode_switch_works_with_all_example_tags) +{ + auto check_perform_command_mode_switch = + [&]( + const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head) + { + TestableResourceManager rm(node_, urdf_head + urdf + ros2_control_test_assets::urdf_tail); + rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + rm.set_component_state("MockHardwareSystem", state); + auto start_interfaces = rm.command_interface_keys(); + std::vector stop_interfaces; + return rm.perform_command_mode_switch(start_interfaces, stop_interfaces); + }; + + ASSERT_TRUE(check_perform_command_mode_switch(hardware_system_2dof_)); + ASSERT_TRUE(check_perform_command_mode_switch(hardware_system_2dof_asymetric_)); + ASSERT_TRUE(check_perform_command_mode_switch(hardware_system_2dof_standard_interfaces_)); + ASSERT_TRUE(check_perform_command_mode_switch(hardware_system_2dof_with_gpio_)); + ASSERT_TRUE(check_perform_command_mode_switch(hardware_system_2dof_with_sensor_)); + ASSERT_TRUE(check_perform_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); + ASSERT_TRUE( + check_perform_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); + ASSERT_TRUE(check_perform_command_mode_switch( + hardware_system_2dof_with_mimic_joint_, ros2_control_test_assets::urdf_head_mimic)); + ASSERT_TRUE( + check_perform_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); + ASSERT_TRUE(check_perform_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_)); + ASSERT_TRUE(check_perform_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_)); + ASSERT_TRUE(check_perform_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_)); + ASSERT_TRUE(check_perform_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_)); + ASSERT_TRUE(check_perform_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_)); + ASSERT_TRUE(check_perform_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_)); + ASSERT_TRUE(check_perform_command_mode_switch(sensor_with_initial_value_)); + ASSERT_TRUE(check_perform_command_mode_switch(gpio_with_initial_value_)); + + ASSERT_TRUE(check_perform_command_mode_switch( + hardware_system_2dof_with_position_control_mode_position_state_only_)); + ASSERT_TRUE(check_perform_command_mode_switch( + hardware_system_2dof_with_velocity_control_mode_position_state_only_)); + ASSERT_TRUE(check_perform_command_mode_switch( + hardware_system_3dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_perform_command_mode_switch(disabled_commands_)); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 1c531124ad..81bcc020dd 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -56,9 +56,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface // BEGIN (Handle export change): for backward compatibility class DummyActuator : public hardware_interface::ActuatorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & /*params*/) override { - // We hardcode the info + // We hardcode the params return CallbackReturn::SUCCESS; } @@ -82,24 +83,28 @@ class DummyActuator : public hardware_interface::ActuatorInterface { // We can read a position and a velocity std::vector state_interfaces; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface( "joint1", hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface( "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_)); - +#pragma GCC diagnostic pop return state_interfaces; } std::vector export_command_interfaces() override { // We can command in velocity +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::vector command_interfaces; command_interfaces.emplace_back( hardware_interface::CommandInterface( "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_)); - +#pragma GCC diagnostic pop return command_interfaces; } @@ -165,11 +170,12 @@ class DummyActuator : public hardware_interface::ActuatorInterface class DummyActuatorDefault : public hardware_interface::ActuatorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - // We hardcode the info + // We hardcode the params if ( - hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::ActuatorInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; @@ -186,6 +192,9 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { set_command("joint1/velocity", 0.0); } + // interfaces are not available + EXPECT_FALSE(has_state("joint1/nonexisting/interface")); + EXPECT_FALSE(has_command("joint1/nonexisting/interface")); // Should throw as the interface is unknown EXPECT_THROW(get_state("joint1/nonexisting/interface"), std::runtime_error); EXPECT_THROW(get_command("joint1/nonexisting/interface"), std::runtime_error); @@ -219,6 +228,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { return hardware_interface::return_type::ERROR; } + EXPECT_TRUE(has_state("joint1/position")); + EXPECT_TRUE(has_state("joint1/velocity")); + EXPECT_FALSE(has_command("joint1/position")); // only velocity command interface + EXPECT_TRUE(has_command("joint1/velocity")); auto position_state = get_state("joint1/position"); set_state("joint1/position", position_state + get_command("joint1/velocity")); set_state("joint1/velocity", get_command("joint1/velocity")); @@ -256,9 +269,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface // BEGIN (Handle export change): for backward compatibility class DummySensor : public hardware_interface::SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & /*params*/) override { - // We hardcode the info + // We hardcode the params return CallbackReturn::SUCCESS; } @@ -273,9 +287,11 @@ class DummySensor : public hardware_interface::SensorInterface { // We can read some voltage level std::vector state_interfaces; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface("sens1", "voltage", &voltage_level_)); - +#pragma GCC diagnostic pop return state_interfaces; } @@ -319,10 +335,11 @@ class DummySensor : public hardware_interface::SensorInterface class DummySensorDefault : public hardware_interface::SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { if ( - hardware_interface::SensorInterface::on_init(info) != + hardware_interface::SensorInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; @@ -334,6 +351,8 @@ class DummySensorDefault : public hardware_interface::SensorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { set_state("sens1/voltage", 0.0); + // interfaces are not available + EXPECT_FALSE(has_state("joint1/nonexisting/interface")); // Should throw as the interface is unknown EXPECT_THROW(get_state("joint1/nonexisting/interface"), std::runtime_error); EXPECT_THROW(set_state("joint1/nonexisting/interface", 0.0), std::runtime_error); @@ -352,6 +371,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface } // no-op, static value + EXPECT_TRUE(has_state("sens1/voltage")); set_state("sens1/voltage", voltage_level_hw_value_); return hardware_interface::return_type::OK; } @@ -380,10 +400,11 @@ class DummySensorDefault : public hardware_interface::SensorInterface class DummySensorJointDefault : public hardware_interface::SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { if ( - hardware_interface::SensorInterface::on_init(info) != + hardware_interface::SensorInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; @@ -441,9 +462,10 @@ class DummySensorJointDefault : public hardware_interface::SensorInterface // BEGIN (Handle export change): for backward compatibility class DummySystem : public hardware_interface::SystemInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & /* params */) override { - // We hardcode the info + // We hardcode the params return CallbackReturn::SUCCESS; } @@ -473,6 +495,8 @@ class DummySystem : public hardware_interface::SystemInterface { // We can read a position and a velocity std::vector state_interfaces; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface( "joint1", hardware_interface::HW_IF_POSITION, &position_state_[0])); @@ -491,7 +515,7 @@ class DummySystem : public hardware_interface::SystemInterface state_interfaces.emplace_back( hardware_interface::StateInterface( "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_state_[2])); - +#pragma GCC diagnostic pop return state_interfaces; } @@ -499,6 +523,8 @@ class DummySystem : public hardware_interface::SystemInterface { // We can command in velocity std::vector command_interfaces; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" command_interfaces.emplace_back( hardware_interface::CommandInterface( "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_[0])); @@ -508,7 +534,7 @@ class DummySystem : public hardware_interface::SystemInterface command_interfaces.emplace_back( hardware_interface::CommandInterface( "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_command_[2])); - +#pragma GCC diagnostic pop return command_interfaces; } @@ -583,10 +609,11 @@ class DummySystem : public hardware_interface::SystemInterface class DummySystemDefault : public hardware_interface::SystemInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { if ( - hardware_interface::SystemInterface::on_init(info) != + hardware_interface::SystemInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; @@ -609,6 +636,9 @@ class DummySystemDefault : public hardware_interface::SystemInterface set_command(velocity_commands_[i], 0.0); } } + // interfaces are not available + EXPECT_FALSE(has_state("joint1/nonexisting/interface")); + EXPECT_FALSE(has_command("joint1/nonexisting/interface")); // Should throw as the interface is unknown EXPECT_THROW(get_state("joint1/nonexisting/interface"), std::runtime_error); EXPECT_THROW(get_command("joint1/nonexisting/interface"), std::runtime_error); @@ -645,6 +675,8 @@ class DummySystemDefault : public hardware_interface::SystemInterface for (size_t i = 0; i < 3; ++i) { + EXPECT_TRUE(has_state(position_states_[i])); + EXPECT_TRUE(has_command(velocity_commands_[i])); auto current_pos = get_state(position_states_[i]); set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); set_state(velocity_states_[i], get_command(velocity_commands_[i])); @@ -692,9 +724,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface class DummySystemPreparePerform : public hardware_interface::SystemInterface { // Override the pure virtual functions with default behavior - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & /* params */) override { - // We hardcode the info + // We hardcode the params return CallbackReturn::SUCCESS; } @@ -753,8 +786,11 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); - auto state = - actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -792,14 +828,13 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read and Write are working because it is INACTIVE + // Read should work but write should not update the state because it is INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ( - step * velocity_value, state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity + EXPECT_EQ(0.0, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -814,9 +849,10 @@ TEST(TestComponentInterfaces, dummy_actuator) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - (10 + step) * velocity_value, - state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value()); // velocity + step * velocity_value, + state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0.0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -830,7 +866,7 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(10 * velocity_value, state_interfaces[0]->get_optional().value()); // position value EXPECT_EQ(0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -855,8 +891,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_actuator; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -915,17 +954,13 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read and Write are working because it is INACTIVE + // Read should work but write should not update the state because it is INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ( - step * velocity_value, - state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0, - state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ(0.0, state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -940,9 +975,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - (10 + step) * velocity_value, + step * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ( + step ? velocity_value : 0.0, + state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -957,7 +994,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - 20 * velocity_value, + 10 * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity @@ -977,8 +1014,11 @@ TEST(TestComponentInterfaces, dummy_sensor) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); - auto state = - sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = sensor_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1017,8 +1057,11 @@ TEST(TestComponentInterfaces, dummy_sensor_default) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = voltage_sensor_res; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = sensor_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1062,8 +1105,11 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) 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()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = sensor_res; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = sensor_hw.initialize(params); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1111,8 +1157,11 @@ TEST(TestComponentInterfaces, dummy_system) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1174,20 +1223,17 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read and Write are working because it is INACTIVE + // Values should 0 because only read should work when INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ( - step * velocity_value, state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity - EXPECT_EQ( - step * velocity_value, state_interfaces[2]->get_optional().value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_optional().value()); // velocity - EXPECT_EQ( - step * velocity_value, state_interfaces[4]->get_optional().value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_optional().value()); // velocity + EXPECT_EQ(0, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_optional().value()); // velocity + EXPECT_EQ(0, state_interfaces[2]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[3]->get_optional().value()); // velocity + EXPECT_EQ(0, state_interfaces[4]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[5]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1202,17 +1248,14 @@ TEST(TestComponentInterfaces, dummy_system) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - (10 + step) * velocity_value, - state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value()); // velocity + step * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity EXPECT_EQ( - (10 + step) * velocity_value, - state_interfaces[2]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_optional().value()); // velocity + step * velocity_value, state_interfaces[2]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_optional().value()); // velocity EXPECT_EQ( - (10 + step) * velocity_value, - state_interfaces[4]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_optional().value()); // velocity + step * velocity_value, state_interfaces[4]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1226,11 +1269,11 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(10 * velocity_value, state_interfaces[0]->get_optional().value()); // position value EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_optional().value()); // position value + EXPECT_EQ(10 * velocity_value, state_interfaces[2]->get_optional().value()); // position value EXPECT_EQ(0.0, state_interfaces[3]->get_optional().value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_optional().value()); // position value + EXPECT_EQ(10 * velocity_value, state_interfaces[4]->get_optional().value()); // position value EXPECT_EQ(0.0, state_interfaces[5]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); @@ -1253,8 +1296,11 @@ TEST(TestComponentInterfaces, dummy_system_default) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_system; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1379,29 +1425,17 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read and Write are working because it is INACTIVE + // Values should 0 because only read should work when INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ( - step * velocity_value, - state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0, - state_interfaces[si_joint1_vel]->get_optional().value()); // velocity - EXPECT_EQ( - step * velocity_value, - state_interfaces[si_joint2_pos]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0, - state_interfaces[si_joint2_vel]->get_optional().value()); // velocity - EXPECT_EQ( - step * velocity_value, - state_interfaces[si_joint3_pos]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0, - state_interfaces[si_joint3_vel]->get_optional().value()); // velocity + EXPECT_EQ(0, state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ(0, state_interfaces[si_joint2_pos]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint2_vel]->get_optional().value()); // velocity + EXPECT_EQ(0, state_interfaces[si_joint3_pos]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint3_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1416,17 +1450,23 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - (10 + step) * velocity_value, + step * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity EXPECT_EQ( - (10 + step) * velocity_value, + step ? velocity_value : 0, + state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint2_pos]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_optional().value()); // velocity EXPECT_EQ( - (10 + step) * velocity_value, + step ? velocity_value : 0, + state_interfaces[si_joint2_vel]->get_optional().value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint3_pos]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_optional().value()); // velocity + EXPECT_EQ( + step ? velocity_value : 0, + state_interfaces[si_joint3_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1441,15 +1481,15 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - 20 * velocity_value, + 10 * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity EXPECT_EQ( - 20 * velocity_value, + 10 * velocity_value, state_interfaces[si_joint2_pos]->get_optional().value()); // position value EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_optional().value()); // velocity EXPECT_EQ( - 20 * velocity_value, + 10 * velocity_value, state_interfaces[si_joint3_pos]->get_optional().value()); // position value EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_optional().value()); // velocity @@ -1466,8 +1506,11 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1500,8 +1543,11 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); - auto state = - actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1569,8 +1615,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_actuator; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1635,8 +1684,11 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); - auto state = - actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1704,8 +1756,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_actuator; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1769,8 +1824,11 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); - auto state = - sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = sensor_hw.initialize(params); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1842,8 +1900,11 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = voltage_sensor_res; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = sensor_hw.initialize(params); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1898,8 +1959,11 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1971,8 +2035,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_system; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -2038,8 +2105,11 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = mock_hw_info; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -2111,8 +2181,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); - auto state = - system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_system; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 70056b528d..80e9ab83f9 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -42,7 +42,6 @@ namespace { const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); -constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; } // namespace using namespace ::testing; // NOLINT @@ -164,8 +163,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_component"); - auto state = - actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_actuator; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = actuator_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -230,8 +232,11 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_component"); - auto state = - sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = voltage_sensor_res; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = sensor_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -267,8 +272,11 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; rclcpp::Node::SharedPtr node = std::make_shared("test_system_component"); - auto state = - system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); + hardware_interface::HardwareComponentParams params; + params.hardware_info = dummy_system; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + auto state = system_hw.initialize(params); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index edc59fefe3..21701b1661 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -15,6 +15,7 @@ #include "gmock/gmock.h" #include "hardware_interface/component_parser.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -879,7 +880,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio 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()); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + ASSERT_EQ(hardware_info.async_params.thread_priority, std::numeric_limits::max()); ASSERT_THAT(hardware_info.joints, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); @@ -951,7 +966,21 @@ 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()); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + ASSERT_EQ(hardware_info.async_params.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)); @@ -980,6 +1009,96 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); } +TEST_F( + TestComponentParser, + successfully_parse_valid_urdf_system_with_bool_data_type_on_joint_sensor_and_gpio) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_robot_with_size_and_data_type_on_joint_sensor_and_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithSizeAndDataType"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType"); + + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_FALSE(hardware_info.is_async); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + ASSERT_EQ(hardware_info.async_params.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(2)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, "enable"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "bool"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + hardware_interface::InterfaceDescription bool_cmd_description( + hardware_info.joints[0].name, hardware_info.joints[0].command_interfaces[1]); + auto bool_cmd_itf = hardware_interface::CommandInterface(bool_cmd_description); + ASSERT_EQ(hardware_interface::HandleDataType::BOOL, bool_cmd_itf.get_data_type()); + + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, "status"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].data_type, "bool"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].size, 1); + hardware_interface::InterfaceDescription bool_state_description( + hardware_info.joints[0].name, hardware_info.joints[0].state_interfaces[1]); + auto bool_state_itf = hardware_interface::StateInterface(bool_state_description); + ASSERT_EQ(hardware_interface::HandleDataType::BOOL, bool_state_itf.get_data_type()); + + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + EXPECT_EQ(hardware_info.sensors[0].name, "trigger"); + EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].name, "safety"); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].size, 1); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.sensors[0].command_interfaces[0].name, "safety"); + EXPECT_EQ(hardware_info.sensors[0].command_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.sensors[0].command_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2); + EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); +} + TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces) { std::string urdf_to_test = @@ -1353,7 +1472,27 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) 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); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + ASSERT_EQ(hardware_info.async_params.thread_priority, 30); + ASSERT_EQ(hardware_info.async_params.scheduling_policy, "detached"); + ASSERT_FALSE(hardware_info.async_params.print_warnings); + ASSERT_EQ(3u, hardware_info.async_params.cpu_affinity_cores.size()); + ASSERT_THAT( + hardware_info.async_params.cpu_affinity_cores, + testing::ContainerEq(std::vector({2, 4, 6}))); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); @@ -1366,7 +1505,24 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) 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); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + ASSERT_EQ(hardware_info.async_params.thread_priority, 50); + ASSERT_EQ(hardware_info.async_params.scheduling_policy, "synchronized"); + ASSERT_TRUE(hardware_info.async_params.print_warnings); + ASSERT_TRUE(hardware_info.async_params.cpu_affinity_cores.empty()); EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); @@ -1390,7 +1546,25 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) 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); + // TODO(anyone): remove this line once thread_priority is removed +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + ASSERT_EQ(hardware_info.async_params.thread_priority, 70); + ASSERT_EQ(hardware_info.async_params.scheduling_policy, "synchronized"); + ASSERT_EQ(1u, hardware_info.async_params.cpu_affinity_cores.size()); + ASSERT_THAT( + hardware_info.async_params.cpu_affinity_cores, testing::ContainerEq(std::vector({1}))); } TEST_F(TestComponentParser, successfully_parse_parameter_empty) diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 06e4e9dc1d..29dc66258b 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -15,7 +15,10 @@ #include #include "gmock/gmock.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include "hardware_interface/handle.hpp" +#pragma GCC diagnostic pop #include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; @@ -29,6 +32,8 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" TEST(TestHandle, command_interface) { double value = 1.337; @@ -37,11 +42,14 @@ TEST(TestHandle, command_interface) ASSERT_TRUE(interface.get_optional().has_value()); EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); - EXPECT_NO_THROW({ interface.set_value(0.0); }); + EXPECT_NO_THROW({ std::ignore = interface.set_value(0.0); }); ASSERT_TRUE(interface.get_optional().has_value()); EXPECT_DOUBLE_EQ(interface.get_optional().value(), 0.0); } +#pragma GCC diagnostic pop +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" TEST(TestHandle, state_interface) { double value = 1.337; @@ -50,10 +58,11 @@ TEST(TestHandle, state_interface) EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); // interface.set_value(5); compiler error, no set_value function } +#pragma GCC diagnostic pop TEST(TestHandle, name_getters_work) { - StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + StateInterface handle{JOINT_NAME, FOO_INTERFACE, nullptr}; EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE)); EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); @@ -61,21 +70,24 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, value_methods_throw_for_nullptr) { - CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; + CommandInterface handle{JOINT_NAME, FOO_INTERFACE, nullptr}; EXPECT_ANY_THROW(handle.get_optional().value()); - EXPECT_ANY_THROW(bool status = handle.set_value(0.0)); + EXPECT_ANY_THROW(std::ignore = handle.set_value(0.0)); } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; ASSERT_TRUE(handle.get_optional().has_value()); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); - EXPECT_NO_THROW({ handle.set_value(0.0); }); + EXPECT_NO_THROW({ std::ignore = handle.set_value(0.0); }); ASSERT_TRUE(handle.get_optional().has_value()); EXPECT_DOUBLE_EQ(handle.get_optional().value(), 0.0); } +#pragma GCC diagnostic pop TEST(TestHandle, test_command_interface_limiter_on_set) { @@ -85,8 +97,8 @@ TEST(TestHandle, test_command_interface_limiter_on_set) info.name = POSITION_INTERFACE; InterfaceDescription interface_descr(JOINT_NAME_1, info); CommandInterface handle{interface_descr}; - handle.set_value(1.0); - EXPECT_DOUBLE_EQ(handle.get_value(), 1.0); + ASSERT_TRUE(handle.set_value(1.0)); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), 1.0); ASSERT_FALSE(handle.is_limited()); handle.set_on_set_command_limiter( @@ -102,15 +114,15 @@ TEST(TestHandle, test_command_interface_limiter_on_set) }); for (int i = 0; i < 10; i++) { - handle.set_limited_value(static_cast(i)); - EXPECT_DOUBLE_EQ(handle.get_value(), i); + ASSERT_TRUE(handle.set_limited_value(static_cast(i))); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), i); ASSERT_FALSE(handle.is_limited()); } for (int i = 11; i < 20; i++) { - handle.set_limited_value(static_cast(i)); - EXPECT_DOUBLE_EQ(handle.get_value(), 10.0); + ASSERT_TRUE(handle.set_limited_value(static_cast(i))); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), 10.0); ASSERT_TRUE(handle.is_limited()); } } @@ -123,8 +135,8 @@ TEST(TestHandle, test_command_interface_limiter_on_set_different_threads) info.name = POSITION_INTERFACE; InterfaceDescription interface_descr(JOINT_NAME_1, info); CommandInterface handle{interface_descr}; - handle.set_value(121.0); - ASSERT_DOUBLE_EQ(handle.get_value(), 121.0); + ASSERT_TRUE(handle.set_value(121.0)); + ASSERT_DOUBLE_EQ(handle.get_optional().value(), 121.0); handle.set_on_set_command_limiter( [](double value, bool & is_limited) -> double @@ -138,8 +150,8 @@ TEST(TestHandle, test_command_interface_limiter_on_set_different_threads) return value; }); - handle.set_limited_value(121.0); - ASSERT_DOUBLE_EQ(handle.get_value(), 100.0); + ASSERT_TRUE(handle.set_limited_value(121.0)); + ASSERT_DOUBLE_EQ(handle.get_optional().value(), 100.0); std::atomic_bool done(false); std::thread checking_thread( @@ -147,10 +159,11 @@ TEST(TestHandle, test_command_interface_limiter_on_set_different_threads) { while (!done) { - double value; - if (handle.get_value(value)) + std::optional opt_value = handle.get_optional(); + + if (opt_value.has_value()) { - EXPECT_DOUBLE_EQ(value, 100.0); + EXPECT_DOUBLE_EQ(opt_value.value(), 100.0); } std::this_thread::sleep_for(std::chrono::microseconds(10)); } @@ -160,7 +173,7 @@ TEST(TestHandle, test_command_interface_limiter_on_set_different_threads) { for (int i = 100; i < 100000; i++) { - handle.set_limited_value(static_cast(i)); + std::ignore = handle.set_limited_value(static_cast(i)); std::this_thread::sleep_for(std::chrono::microseconds(10)); } done = true; @@ -169,7 +182,7 @@ TEST(TestHandle, test_command_interface_limiter_on_set_different_threads) modifier_thread.join(); done = true; checking_thread.join(); - EXPECT_DOUBLE_EQ(handle.get_value(), 100.0); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), 100.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) @@ -186,11 +199,11 @@ TEST(TestHandle, interface_description_state_interface_name_getters_work) EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); - EXPECT_NO_THROW({ handle.get_optional(); }); + EXPECT_NO_THROW({ std::ignore = handle.get_optional(); }); ASSERT_TRUE(handle.get_optional().has_value()); - ASSERT_THROW({ handle.get_optional(); }, std::runtime_error); - ASSERT_THROW({ handle.set_value(true); }, std::runtime_error); + ASSERT_THROW({ std::ignore = handle.get_optional(); }, std::runtime_error); + ASSERT_THROW({ std::ignore = handle.set_value(true); }, std::runtime_error); } TEST(TestHandle, interface_description_bool_data_type) @@ -208,17 +221,76 @@ TEST(TestHandle, interface_description_bool_data_type) EXPECT_EQ(handle.get_name(), itf_name + "/" + collision_interface); EXPECT_EQ(handle.get_interface_name(), collision_interface); EXPECT_EQ(handle.get_prefix_name(), itf_name); - EXPECT_NO_THROW({ handle.get_optional(); }); + EXPECT_NO_THROW({ std::ignore = handle.get_optional(); }); ASSERT_FALSE(handle.get_optional().value()) << "Default value should be false"; - EXPECT_NO_THROW({ handle.set_value(true); }); + ASSERT_TRUE(handle.set_value(true)); ASSERT_TRUE(handle.get_optional().value()); - EXPECT_NO_THROW({ handle.set_value(false); }); + ASSERT_EQ(handle.get_optional(), 1.0); + ASSERT_TRUE(handle.set_value(false)); + ASSERT_FALSE(handle.get_optional().value()); + ASSERT_EQ(handle.get_optional(), 0.0); + + info.name = "some_interface"; + interface_descr = InterfaceDescription(itf_name, info); + StateInterface handle2{interface_descr}; + EXPECT_EQ(handle2.get_name(), itf_name + "/" + "some_interface"); + ASSERT_TRUE(handle2.set_value(false)); + ASSERT_FALSE(handle2.get_optional().value()); + ASSERT_EQ(handle2.get_optional(), 0.0); + + // Test the assertions + ASSERT_THROW({ std::ignore = handle.set_value(-1.0); }, std::runtime_error); + ASSERT_THROW({ std::ignore = handle.set_value(0.0); }, std::runtime_error); +} + +TEST(TestHandle, handle_constructor_double_data_type) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + StateInterface handle{JOINT_NAME_1, POSITION_INTERFACE, "double", "23.0"}; + + ASSERT_EQ(hardware_interface::HandleDataType::DOUBLE, handle.get_data_type()); + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); + EXPECT_NO_THROW({ std::ignore = handle.get_optional(); }); + ASSERT_EQ(handle.get_optional().value(), 23.0); + ASSERT_TRUE(handle.get_optional().has_value()); + ASSERT_TRUE(handle.set_value(0.0)); + ASSERT_EQ(handle.get_optional().value(), 0.0); + ASSERT_TRUE(handle.set_value(1.0)); + ASSERT_EQ(handle.get_optional().value(), 1.0); + + // Test the assertions + ASSERT_THROW({ std::ignore = handle.get_optional(); }, std::runtime_error); + ASSERT_THROW({ std::ignore = handle.set_value(true); }, std::runtime_error); + EXPECT_ANY_THROW({ StateInterface bad_itf("joint1", POSITION_INTERFACE, "double", "233NaN0"); }) + << "Invalid double value should throw"; +} + +TEST(TestHandle, handle_constructor_bool_data_type) +{ + const std::string collision_interface = "collision"; + const std::string itf_name = "joint1"; + StateInterface handle{itf_name, collision_interface, "bool", "true"}; + + ASSERT_EQ(hardware_interface::HandleDataType::BOOL, handle.get_data_type()); + EXPECT_EQ(handle.get_name(), itf_name + "/" + collision_interface); + EXPECT_EQ(handle.get_interface_name(), collision_interface); + EXPECT_EQ(handle.get_prefix_name(), itf_name); + EXPECT_NO_THROW({ std::ignore = handle.get_optional(); }); + ASSERT_TRUE(handle.get_optional().value()) + << "Default value should be true as it is initialized"; + ASSERT_TRUE(handle.set_value(false)); ASSERT_FALSE(handle.get_optional().value()); + ASSERT_EQ(handle.get_optional(), 0.0); + ASSERT_TRUE(handle.set_value(true)); + ASSERT_TRUE(handle.get_optional().value()); + ASSERT_EQ(handle.get_optional(), 1.0); // Test the assertions - ASSERT_THROW({ handle.set_value(-1.0); }, std::runtime_error); - ASSERT_THROW({ handle.set_value(0.0); }, std::runtime_error); - ASSERT_THROW({ handle.get_optional(); }, std::runtime_error); + ASSERT_THROW({ std::ignore = handle.set_value(-1.0); }, std::runtime_error); + ASSERT_THROW({ std::ignore = handle.set_value(0.0); }, std::runtime_error); } TEST(TestHandle, interface_description_unknown_data_type) @@ -232,6 +304,8 @@ TEST(TestHandle, interface_description_unknown_data_type) ASSERT_EQ(hardware_interface::HandleDataType::UNKNOWN, interface_descr.get_data_type()); EXPECT_ANY_THROW({ StateInterface handle{interface_descr}; }) << "Unknown data type should throw"; + EXPECT_ANY_THROW({ StateInterface handle("joint1", "collision", "UNKNOWN"); }) + << "Unknown data type should throw"; } TEST(TestHandle, interface_description_command_interface_name_getters_work) @@ -247,7 +321,8 @@ TEST(TestHandle, interface_description_command_interface_name_getters_work) EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); } - +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" TEST(TestHandle, copy_constructor) { { @@ -256,7 +331,7 @@ TEST(TestHandle, copy_constructor) hardware_interface::Handle copy(handle); EXPECT_DOUBLE_EQ(copy.get_optional().value(), value); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); - EXPECT_NO_THROW({ copy.set_value(0.0); }); + ASSERT_TRUE(copy.set_value(0.0)); EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); EXPECT_DOUBLE_EQ(handle.get_optional().value(), 0.0); } @@ -275,10 +350,10 @@ TEST(TestHandle, copy_constructor) EXPECT_EQ(copy.get_prefix_name(), handle.get_prefix_name()); EXPECT_DOUBLE_EQ(copy.get_optional().value(), value); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); - EXPECT_NO_THROW({ copy.set_value(0.0); }); + ASSERT_TRUE(copy.set_value(0.0)); EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); - EXPECT_NO_THROW({ copy.set_value(0.52); }); + ASSERT_TRUE(copy.set_value(0.52)); EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.52); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); } @@ -290,7 +365,7 @@ TEST(TesHandle, move_constructor) hardware_interface::Handle handle{JOINT_NAME, FOO_INTERFACE, &value}; hardware_interface::Handle moved{std::move(handle)}; EXPECT_DOUBLE_EQ(moved.get_optional().value(), value); - EXPECT_NO_THROW({ moved.set_value(0.0); }); + ASSERT_TRUE(moved.set_value(0.0)); EXPECT_DOUBLE_EQ(moved.get_optional().value(), 0.0); } @@ -306,7 +381,7 @@ TEST(TestHandle, copy_assignment) copy = handle; EXPECT_DOUBLE_EQ(copy.get_optional().value(), value_1); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value_1); - EXPECT_NO_THROW({ copy.set_value(0.0); }); + ASSERT_TRUE(copy.set_value(0.0)); EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); EXPECT_DOUBLE_EQ(handle.get_optional().value(), 0.0); EXPECT_DOUBLE_EQ(value_1, 0.0); @@ -328,10 +403,10 @@ TEST(TestHandle, copy_assignment) EXPECT_EQ(copy.get_prefix_name(), handle.get_prefix_name()); EXPECT_DOUBLE_EQ(copy.get_optional().value(), value); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); - EXPECT_NO_THROW({ copy.set_value(0.0); }); + ASSERT_TRUE(copy.set_value(0.0)); EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); - EXPECT_NO_THROW({ copy.set_value(0.52); }); + ASSERT_TRUE(copy.set_value(0.52)); EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.52); EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); } @@ -347,6 +422,7 @@ TEST(TestHandle, move_assignment) EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); moved = std::move(handle); EXPECT_DOUBLE_EQ(moved.get_optional().value(), value); - EXPECT_NO_THROW({ moved.set_value(0.0); }); + ASSERT_TRUE(moved.set_value(0.0)); EXPECT_DOUBLE_EQ(moved.get_optional().value(), 0.0); } +#pragma GCC diagnostic pop diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index acec0e5917..e973ecfa10 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -26,9 +26,10 @@ namespace test_hardware_components { class TestForceTorqueSensor : public SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & sensor_info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (SensorInterface::on_init(sensor_info) != CallbackReturn::SUCCESS) + if (SensorInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -58,6 +59,8 @@ class TestForceTorqueSensor : public SensorInterface std::vector state_interfaces; const auto & sensor_name = get_hardware_info().sensors[0].name; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); state_interfaces.emplace_back( @@ -70,7 +73,7 @@ class TestForceTorqueSensor : public SensorInterface hardware_interface::StateInterface(sensor_name, "ty", &values_.ty)); state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "tz", &values_.tz)); - +#pragma GCC diagnostic pop return state_interfaces; } diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index 31b9419191..b627ce6b59 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -31,9 +31,10 @@ namespace test_hardware_components { class TestIMUSensor : public SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & sensor_info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (SensorInterface::on_init(sensor_info) != CallbackReturn::SUCCESS) + if (SensorInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -66,6 +67,8 @@ class TestIMUSensor : public SensorInterface std::vector state_interfaces; const std::string & sensor_name = get_hardware_info().sensors[0].name; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); state_interfaces.emplace_back( @@ -89,7 +92,7 @@ class TestIMUSensor : public SensorInterface state_interfaces.emplace_back( hardware_interface::StateInterface( sensor_name, "linear_acceleration.z", &linear_acceleration_.z)); - +#pragma GCC diagnostic pop return state_interfaces; } diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 62c1431806..53ddc05dcc 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -26,9 +26,10 @@ namespace test_hardware_components { class TestSingleJointActuator : public ActuatorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & actuator_info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (ActuatorInterface::on_init(actuator_info) != CallbackReturn::SUCCESS) + if (ActuatorInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -72,13 +73,15 @@ class TestSingleJointActuator : public ActuatorInterface std::vector state_interfaces; const auto & joint_name = get_hardware_info().joints[0].name; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_VELOCITY, &velocity_state_)); - +#pragma GCC diagnostic pop return state_interfaces; } @@ -87,9 +90,12 @@ class TestSingleJointActuator : public ActuatorInterface std::vector command_interfaces; const auto & joint_name = get_hardware_info().joints[0].name; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" command_interfaces.emplace_back( hardware_interface::CommandInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); +#pragma GCC diagnostic pop return command_interfaces; } diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index 3e020d8681..6a9b320bcf 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -24,9 +24,10 @@ namespace test_hardware_components class TestSystemCommandModes : public hardware_interface::SystemInterface { public: - CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -79,6 +80,8 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector state_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" state_interfaces.emplace_back( hardware_interface::StateInterface( get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, @@ -91,6 +94,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface hardware_interface::StateInterface( get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); +#pragma GCC diagnostic pop } return state_interfaces; @@ -101,6 +105,8 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector command_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" command_interfaces.emplace_back( hardware_interface::CommandInterface( get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, @@ -109,6 +115,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface hardware_interface::CommandInterface( get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); +#pragma GCC diagnostic pop } return command_interfaces; diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 9a3cc66e9c..c5b1cf74bb 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -27,9 +27,10 @@ namespace test_hardware_components { class TestTwoJointSystem : public SystemInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) + if (SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -67,6 +68,8 @@ class TestTwoJointSystem : public SystemInterface return CallbackReturn::SUCCESS; } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::vector export_state_interfaces() override { std::vector state_interfaces; @@ -80,12 +83,17 @@ class TestTwoJointSystem : public SystemInterface return state_interfaces; } +#pragma GCC diagnostic pop +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::vector export_command_interfaces() override { std::vector command_interfaces; for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" command_interfaces.emplace_back( hardware_interface::CommandInterface( get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, @@ -94,6 +102,7 @@ class TestTwoJointSystem : public SystemInterface return command_interfaces; } +#pragma GCC diagnostic pop return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { diff --git a/hardware_interface/test/test_lexical_casts.cpp b/hardware_interface/test/test_lexical_casts.cpp new file mode 100644 index 0000000000..3300ea1554 --- /dev/null +++ b/hardware_interface/test/test_lexical_casts.cpp @@ -0,0 +1,156 @@ +// Copyright 2025 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "gtest/gtest.h" + +#include "hardware_interface/lexical_casts.hpp" + +TEST(TestLexicalCasts, test_stod) +{ + using hardware_interface::stod; + + ASSERT_THROW(stod(""), std::invalid_argument); + ASSERT_THROW(stod("abc"), std::invalid_argument); + ASSERT_THROW(stod("1.2.3"), std::invalid_argument); + ASSERT_EQ(stod("1.2"), 1.2); + ASSERT_EQ(stod("-1.2"), -1.2); + ASSERT_EQ(stod("1.0"), 1.0); +} + +TEST(TestLexicalCasts, test_parse_bool) +{ + using hardware_interface::parse_bool; + + ASSERT_TRUE(parse_bool("true")); + ASSERT_TRUE(parse_bool("True")); + ASSERT_TRUE(parse_bool("TRUE")); + ASSERT_TRUE(parse_bool("TrUe")); + + // Any other value should return false + ASSERT_FALSE(parse_bool("false")); + ASSERT_FALSE(parse_bool("False")); + ASSERT_FALSE(parse_bool("FALSE")); + ASSERT_FALSE(parse_bool("fAlSe")); + + // Invalid inputs should throw an exception + ASSERT_THROW(parse_bool("1"), std::invalid_argument); + ASSERT_THROW(parse_bool("0"), std::invalid_argument); + ASSERT_THROW(parse_bool("yes"), std::invalid_argument); + ASSERT_THROW(parse_bool("no"), std::invalid_argument); + ASSERT_THROW(parse_bool(""), std::invalid_argument); + ASSERT_THROW(parse_bool("abc"), std::invalid_argument); + ASSERT_THROW(parse_bool("1"), std::invalid_argument); +} + +TEST(TestLexicalCasts, test_parse_string_array) +{ + using hardware_interface::parse_string_array; + + ASSERT_THROW(parse_string_array(""), std::invalid_argument); + ASSERT_THROW(parse_string_array("abc"), std::invalid_argument); + ASSERT_THROW(parse_string_array("[abc"), std::invalid_argument); + ASSERT_THROW(parse_string_array("abc]"), std::invalid_argument); + ASSERT_THROW(parse_string_array("[[abc, def], hij]"), std::invalid_argument); + ASSERT_THROW(parse_string_array("[,]"), std::invalid_argument); + ASSERT_THROW(parse_string_array("[abc,]"), std::invalid_argument); + ASSERT_THROW(parse_string_array("[,abc]"), std::invalid_argument); + ASSERT_THROW(parse_string_array("[abc,,def]"), std::invalid_argument); + + ASSERT_EQ(parse_string_array("[]"), std::vector()); + ASSERT_EQ(parse_string_array("[ ]"), std::vector()); + ASSERT_EQ(parse_string_array("[abc]"), std::vector({"abc"})); + ASSERT_EQ(parse_string_array("[abc,def]"), std::vector({"abc", "def"})); + ASSERT_EQ(parse_string_array("[abc, def]"), std::vector({"abc", "def"})); + ASSERT_EQ(parse_string_array("[ abc, def ]"), std::vector({"abc", "def"})); +} + +TEST(TestLexicalCasts, test_parse_double_array) +{ + using hardware_interface::parse_array; + + ASSERT_THROW(parse_array(""), std::invalid_argument); + ASSERT_THROW(parse_array("1.23"), std::invalid_argument); + ASSERT_THROW(parse_array("[1.23"), std::invalid_argument); + ASSERT_THROW(parse_array("1.23]"), std::invalid_argument); + ASSERT_THROW(parse_array("[[1.23, 4.56], 7.89]"), std::invalid_argument); + ASSERT_THROW(parse_array("[,]"), std::invalid_argument); + ASSERT_THROW(parse_array("[ , ]"), std::invalid_argument); + ASSERT_THROW(parse_array("[1.23,]"), std::invalid_argument); + ASSERT_THROW(parse_array("[,1.234]"), std::invalid_argument); + ASSERT_THROW(parse_array("[1.232,,1.324]"), std::invalid_argument); + + ASSERT_EQ(parse_array("[]"), std::vector()); + ASSERT_EQ(parse_array("[ ]"), std::vector()); + ASSERT_EQ(parse_array("[1.23]"), std::vector({1.23})); + ASSERT_EQ(parse_array("[-1.23]"), std::vector({-1.23})); + ASSERT_EQ(parse_array("[1.23,4.56]"), std::vector({1.23, 4.56})); + ASSERT_EQ(parse_array("[-1.23,-4.56]"), std::vector({-1.23, -4.56})); + ASSERT_EQ(parse_array("[-1.23, 4.56]"), std::vector({-1.23, 4.56})); + ASSERT_EQ(parse_array("[ -1.23, -124.56 ]"), std::vector({-1.23, -124.56})); + ASSERT_EQ(parse_array("[ 1.23, 4 ]"), std::vector({1.23, 4.0})); + ASSERT_EQ(parse_array("[ 1.23, 4.56, -7 ]"), std::vector({1.23, 4.56, -7.0})); + ASSERT_EQ(parse_array("[ 1.23, 123, -7.89 ]"), std::vector({1.23, 123.0, -7.89})); + ASSERT_EQ(parse_array("[ 1.23, 4.56, -7.89 ]"), std::vector({1.23, 4.56, -7.89})); +} + +TEST(TestLexicalCasts, test_parse_int_array) +{ + using hardware_interface::parse_array; + + ASSERT_THROW(parse_array(""), std::invalid_argument); + ASSERT_THROW(parse_array("123"), std::invalid_argument); + ASSERT_THROW(parse_array("[232"), std::invalid_argument); + ASSERT_THROW(parse_array("123]"), std::invalid_argument); + ASSERT_THROW(parse_array("[[1.23, 4], 7]"), std::invalid_argument); + ASSERT_THROW(parse_array("[,]"), std::invalid_argument); + ASSERT_THROW(parse_array("[ , ]"), std::invalid_argument); + ASSERT_THROW(parse_array("[1,]"), std::invalid_argument); + ASSERT_THROW(parse_array("[,1]"), std::invalid_argument); + ASSERT_THROW(parse_array("[1,,2]"), std::invalid_argument); + + ASSERT_EQ(parse_array("[]"), std::vector()); + ASSERT_EQ(parse_array("[ ]"), std::vector()); + ASSERT_EQ(parse_array("[1]"), std::vector({1})); + ASSERT_EQ(parse_array("[-1]"), std::vector({-1})); + ASSERT_EQ(parse_array("[1,2]"), std::vector({1, 2})); + ASSERT_EQ(parse_array("[-1,-2]"), std::vector({-1, -2})); + ASSERT_EQ(parse_array("[ -1, -124 ]"), std::vector({-1, -124})); + ASSERT_EQ(parse_array("[ -1, -124, +123 ]"), std::vector({-1, -124, 123})); +} + +TEST(TestLexicalCasts, test_parse_bool_array) +{ + using hardware_interface::parse_array; + + ASSERT_THROW(parse_array(""), std::invalid_argument); + ASSERT_THROW(parse_array("true"), std::invalid_argument); + ASSERT_THROW(parse_array("[true"), std::invalid_argument); + ASSERT_THROW(parse_array("true]"), std::invalid_argument); + ASSERT_THROW(parse_array("[[true, false], true]"), std::invalid_argument); + ASSERT_THROW(parse_array("[,]"), std::invalid_argument); + ASSERT_THROW(parse_array("[ , ]"), std::invalid_argument); + ASSERT_THROW(parse_array("[true,]"), std::invalid_argument); + ASSERT_THROW(parse_array("[,false]"), std::invalid_argument); + ASSERT_THROW(parse_array("[true,,false]"), std::invalid_argument); + + ASSERT_EQ(parse_array("[]"), std::vector()); + ASSERT_EQ(parse_array("[ ]"), std::vector()); + ASSERT_EQ(parse_array("[true]"), std::vector({true})); + ASSERT_EQ(parse_array("[false]"), std::vector({false})); + ASSERT_EQ(parse_array("[true,false]"), std::vector({true, false})); + ASSERT_EQ(parse_array("[false,true]"), std::vector({false, true})); + ASSERT_EQ(parse_array("[ true, false ]"), std::vector({true, false})); +} diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 6e51142769..1f22e523f6 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,57 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ + +5.7.0 (2025-10-03) +------------------ +* Cleanup deprecations for kilted release (`#2605 `_) +* Add parameter to allow controllers with inactive hardware components (`#2501 `_) +* Fix exclusive hardware control mode switching on controller failed activation (`#1522 `_) +* Fix shadowed variables, redefinition and old-style casts (`#2569 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +5.6.0 (2025-08-26) +------------------ +* Unify `write` behavior between Actuator and System hardware interfaces (`#2453 `_) +* Supress deprecated RM API warnings in the tests (`#2428 `_) +* Contributors: Sai Kishor Kothakota, Soham Patil + +5.5.0 (2025-07-31) +------------------ +* Fix the prepare_command_mode_switch behaviour when HW is INACTIVE (`#2347 `_) +* Contributors: Sai Kishor Kothakota + +5.4.0 (2025-07-21) +------------------ +* Addition of a Default Node for Hardware Component (`#2348 `_) +* Contributors: Soham Patil + +5.3.0 (2025-07-02) +------------------ +* Add deprecations to old methods not using param structs (`#2344 `_) +* Deactivate controllers with command interfaces to hardware on DEACTIVATE (`#2334 `_) +* Contributors: Marq Rasmussen, Sai Kishor Kothakota + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ +* [RM] Isolate start and stop interfaces for each Hardware Component (`#2120 `_) +* Use target_link_libraries instead of ament_target_dependencies (`#2266 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* Statically allocate string concatenations using FMT formatting (`#2205 `_) +* Suppress the deprecation warnings of the hardware_interface API (`#2223 `_) +* Contributors: Sai Kishor Kothakota, mini-1235 + 4.29.0 (2025-05-04) ------------------- * [Diagnostics] Add diagnostics of execution time and periodicity of the hardware components (`#2086 `_) diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index 2345f0af88..fabef98ae6 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -6,12 +6,12 @@ set_compiler_options() export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs hardware_interface lifecycle_msgs pluginlib rclcpp_lifecycle ros2_control_test_assets + fmt ) find_package(ament_cmake REQUIRED) @@ -20,11 +20,17 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(test_components SHARED -test/test_components/test_actuator.cpp -test/test_components/test_sensor.cpp -test/test_components/test_system.cpp -test/test_components/test_actuator_exclusive_interfaces.cpp) -ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets) + test/test_components/test_actuator.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp + test/test_components/test_actuator_exclusive_interfaces.cpp) + +target_link_libraries(test_components + hardware_interface::hardware_interface + rclcpp_lifecycle::rclcpp_lifecycle + ros2_control_test_assets::ros2_control_test_assets + pluginlib::pluginlib + fmt::fmt) install(TARGETS test_components DESTINATION lib ) @@ -36,10 +42,18 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) - ament_target_dependencies(test_resource_manager hardware_interface ros2_control_test_assets) + target_link_libraries(test_resource_manager + hardware_interface::hardware_interface + rclcpp_lifecycle::rclcpp_lifecycle + ros2_control_test_assets::ros2_control_test_assets + ${lifecycle_msgs_TARGETS}) ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp) - ament_target_dependencies(test_resource_manager_prepare_perform_switch hardware_interface ros2_control_test_assets) + target_link_libraries(test_resource_manager_prepare_perform_switch + hardware_interface::hardware_interface + rclcpp_lifecycle::rclcpp_lifecycle + ros2_control_test_assets::ros2_control_test_assets + ${lifecycle_msgs_TARGETS}) endif() diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index a11d492a26..82c8734ae9 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.29.0 + 6.0.1 Commonly used test fixtures for the ros2_control framework Bence Magyar Denis Štogl @@ -18,6 +18,7 @@ pluginlib rclcpp_lifecycle ros2_control_test_assets + fmt ament_cmake_gmock diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 8ee4afa101..83a5eeba36 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -25,9 +25,10 @@ using hardware_interface::StateInterface; class TestActuator : public ActuatorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) + if (ActuatorInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -182,9 +183,10 @@ class TestActuator : public ActuatorInterface class TestUninitializableActuator : public TestActuator { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - ActuatorInterface::on_init(info); + ActuatorInterface::on_init(params); return CallbackReturn::ERROR; } }; diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp index bc330d2e95..5b85c96056 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -43,14 +43,15 @@ struct JointState class TestActuatorExclusiveInterfaces : public ActuatorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) + if (ActuatorInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } - for (const auto & j : info.joints) + for (const auto & j : get_hardware_info().joints) { (void)j; // Suppress unused warning current_states_.emplace_back(JointState{}); diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index b3ae421601..31a3dd3e9b 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -5,6 +5,12 @@ Test Actuator + + + Test Actuator + + + diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 5bc8b6ae80..a0445e6365 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -23,9 +23,10 @@ using hardware_interface::StateInterface; class TestSensor : public SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (SensorInterface::on_init(info) != CallbackReturn::SUCCESS) + if (SensorInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -67,9 +68,10 @@ class TestSensor : public SensorInterface class TestUninitializableSensor : public TestSensor { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - SensorInterface::on_init(info); + SensorInterface::on_init(params); return CallbackReturn::ERROR; } }; diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index dbbc2183fc..e27d0aebfd 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -27,9 +27,10 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + if (SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -170,9 +171,10 @@ class TestSystem : public SystemInterface class TestUninitializableSystem : public TestSystem { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override { - SystemInterface::on_init(info); + SystemInterface::on_init(params); return CallbackReturn::ERROR; } }; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index f252478002..f7b75faed8 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -99,14 +99,20 @@ TEST_F(ResourceManagerTest, initialization_with_urdf) TEST_F(ResourceManagerTest, post_initialization_with_urdf) { TestableResourceManager rm(node_); - ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); + hardware_interface::ResourceManagerParams rm_params; + rm_params.robot_description = ros2_control_test_assets::minimal_robot_urdf; + rm_params.update_rate = 100; + ASSERT_NO_THROW(rm.load_and_initialize_components(rm_params)); } void test_load_and_initialized_components_failure(const std::string & urdf) { rclcpp::Node node = rclcpp::Node("TestableResourceManager"); TestableResourceManager rm(node); - ASSERT_FALSE(rm.load_and_initialize_components(urdf)); + hardware_interface::ResourceManagerParams rm_params; + rm_params.robot_description = urdf; + rm_params.update_rate = 100; + ASSERT_NO_THROW(rm.load_and_initialize_components(rm_params)); ASSERT_FALSE(rm.are_components_initialized()); @@ -265,14 +271,6 @@ TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_async_urdf_ ASSERT_TRUE(rm.are_components_initialized()); } -TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) -{ - TestableResourceManager rm(node_); - ASSERT_FALSE(rm.are_components_initialized()); - rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); - ASSERT_TRUE(rm.are_components_initialized()); -} - TEST_F(ResourceManagerTest, resource_claiming) { TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); @@ -403,7 +401,9 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) external_component_hw_info.name = "ExternalComponent"; external_component_hw_info.type = "actuator"; external_component_hw_info.is_async = false; - rm.import_component(std::make_unique(), external_component_hw_info); + hardware_interface::HardwareComponentParams params; + params.hardware_info = external_component_hw_info; + rm.import_component(std::make_unique(), params); EXPECT_EQ(2u, rm.actuator_components_size()); ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); @@ -1289,8 +1289,8 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) EXPECT_EQ(claimed_itf1.get_optional().value(), 1.0); EXPECT_EQ(claimed_itf3.get_optional().value(), 3.0); - claimed_itf1.set_value(11.1); - claimed_itf3.set_value(33.3); + ASSERT_TRUE(claimed_itf1.set_value(11.1)); + ASSERT_TRUE(claimed_itf3.set_value(33.3)); EXPECT_EQ(claimed_itf1.get_optional().value(), 11.1); EXPECT_EQ(claimed_itf3.get_optional().value(), 33.3); @@ -1340,6 +1340,49 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } +class MockExecutor : public rclcpp::executors::SingleThreadedExecutor +{ +public: + explicit MockExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()) + : rclcpp::executors::SingleThreadedExecutor(options) + { + } + + void add_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) override + { + rclcpp::executors::SingleThreadedExecutor::add_node(node_ptr, notify); + added_node_names.push_back(node_ptr->get_name()); + } + + std::vector added_node_names; +}; + +TEST_F(ResourceManagerTest, hardware_nodes_are_added_to_mock_executor_on_load) +{ + auto mock_executor = std::make_shared(); + + hardware_interface::ResourceManagerParams params; + params.robot_description = ros2_control_test_assets::minimal_robot_urdf; + params.clock = node_.get_clock(); + params.logger = node_.get_logger(); + params.executor = mock_executor; + TestableResourceManager rm(params); + auto to_lower = [](const std::string & s) + { + std::string result = s; + std::transform( + result.begin(), result.end(), result.begin(), + [](unsigned char c) { return std::tolower(c); }); + return result; + }; + EXPECT_THAT( + mock_executor->added_node_names, + testing::UnorderedElementsAre( + to_lower(TEST_ACTUATOR_HARDWARE_NAME), to_lower(TEST_SENSOR_HARDWARE_NAME), + to_lower(TEST_SYSTEM_HARDWARE_NAME))); +} + class ResourceManagerTestReadWriteError : public ResourceManagerTest { public: @@ -1367,13 +1410,13 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest 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); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); } { - auto [ok, failed_hardware_names] = rm->write(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->write(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); } check_if_interface_available(true, true); @@ -1412,11 +1455,11 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest hardware_interface::lifecycle_state_names::ACTIVE); // read failure for TEST_ACTUATOR_HARDWARE_NAME - claimed_itfs[0].set_value(fail_value); - claimed_itfs[1].set_value(fail_value - 10.0); + ASSERT_TRUE(claimed_itfs[0].set_value(fail_value)); + ASSERT_TRUE(claimed_itfs[1].set_value(fail_value - 10.0)); { - auto [ok, failed_hardware_names] = method_that_fails(time, duration); - EXPECT_FALSE(ok); + auto [result, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::ERROR); EXPECT_FALSE(failed_hardware_names.empty()); ASSERT_THAT( failed_hardware_names, @@ -1441,18 +1484,18 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } // write is sill OK { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = other_method(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); check_if_interface_available(true, true); } // read failure for TEST_SYSTEM_HARDWARE_NAME - claimed_itfs[0].set_value(fail_value - 10.0); - claimed_itfs[1].set_value(fail_value); + ASSERT_TRUE(claimed_itfs[0].set_value(fail_value - 10.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(fail_value)); { - auto [ok, failed_hardware_names] = method_that_fails(time, duration); - EXPECT_FALSE(ok); + auto [result, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::ERROR); EXPECT_FALSE(failed_hardware_names.empty()); ASSERT_THAT( failed_hardware_names, @@ -1477,19 +1520,19 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } // write is sill OK { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = other_method(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); check_if_interface_available(true, true); } // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and // TEST_SYSTEM_HARDWARE_NAME - claimed_itfs[0].set_value(fail_value); - claimed_itfs[1].set_value(fail_value); + ASSERT_TRUE(claimed_itfs[0].set_value(fail_value)); + ASSERT_TRUE(claimed_itfs[1].set_value(fail_value)); { - auto [ok, failed_hardware_names] = method_that_fails(time, duration); - EXPECT_FALSE(ok); + auto [result, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::ERROR); EXPECT_FALSE(failed_hardware_names.empty()); ASSERT_THAT( failed_hardware_names, @@ -1516,14 +1559,14 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } // write is sill OK { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = other_method(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); check_if_interface_available(true, true); } } - void check_read_or_write_deactivate( + void check_write_deactivate( FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) { // define state to set components to @@ -1532,13 +1575,13 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest hardware_interface::lifecycle_state_names::ACTIVE); // deactivate for TEST_ACTUATOR_HARDWARE_NAME - claimed_itfs[0].set_value(deactivate_value); - claimed_itfs[1].set_value(deactivate_value - 10.0); + ASSERT_TRUE(claimed_itfs[0].set_value(deactivate_value)); + ASSERT_TRUE(claimed_itfs[1].set_value(deactivate_value - 10.0)); { // deactivate on error - auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + auto [result, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::DEACTIVATE); + EXPECT_TRUE(!failed_hardware_names.empty()); auto status_map = rm->get_components_status(); EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -1561,20 +1604,20 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } // write is sill OK { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = other_method(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); check_if_interface_available(true, true); } // deactivate for TEST_SYSTEM_HARDWARE_NAME - claimed_itfs[0].set_value(deactivate_value - 10.0); - claimed_itfs[1].set_value(deactivate_value); + ASSERT_TRUE(claimed_itfs[0].set_value(deactivate_value - 10.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(deactivate_value)); { // deactivate on flag - auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + auto [result, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::DEACTIVATE); + EXPECT_TRUE(!failed_hardware_names.empty()); auto status_map = rm->get_components_status(); EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -1596,21 +1639,21 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } // write is sill OK { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = other_method(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); check_if_interface_available(true, true); } // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and // TEST_SYSTEM_HARDWARE_NAME - claimed_itfs[0].set_value(deactivate_value); - claimed_itfs[1].set_value(deactivate_value); + ASSERT_TRUE(claimed_itfs[0].set_value(deactivate_value)); + ASSERT_TRUE(claimed_itfs[1].set_value(deactivate_value)); { // deactivate on flag - auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + auto [result, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::DEACTIVATE); + EXPECT_TRUE(!failed_hardware_names.empty()); auto status_map = rm->get_components_status(); EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -1633,8 +1676,8 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } // write is sill OK { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = other_method(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); check_if_interface_available(true, true); } @@ -1677,8 +1720,8 @@ TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) setup_resource_manager_and_do_initial_checks(); using namespace std::placeholders; - // check read methods failures - check_read_or_write_deactivate( + // check read methods failures (DEACTIVATE is the same as ERROR on read) + check_read_or_write_failure( std::bind(&TestableResourceManager::read, rm, _1, _2), std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); } @@ -1689,7 +1732,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) using namespace std::placeholders; // check write methods failures - check_read_or_write_deactivate( + check_write_deactivate( std::bind(&TestableResourceManager::write, rm, _1, _2), std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); } @@ -1792,15 +1835,15 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage 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); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::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); + ASSERT_TRUE(claimed_itfs[0].set_value(10.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(20.0)); + auto [result, failed_hardware_names] = rm->write(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); } @@ -1832,27 +1875,27 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage using FunctionT = std::function; - void check_read_and_write_cycles(bool test_for_changing_values) + void check_read_and_write_cycles(bool test_for_changing_values, bool is_write_active) { double prev_act_state_value = state_itfs[0].get_optional().value(); double prev_system_state_value = state_itfs[1].get_optional().value(); for (size_t i = 1; i < 100; i++) { - auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::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_optional().value() / 2.0; - claimed_itfs[1].set_value(claimed_itfs[1].get_optional().value() + 20.0); + ASSERT_TRUE(claimed_itfs[1].set_value(claimed_itfs[1].get_optional().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_optional().value() / 2.0; - claimed_itfs[0].set_value(claimed_itfs[0].get_optional().value() + 10.0); + ASSERT_TRUE(claimed_itfs[0].set_value(claimed_itfs[0].get_optional().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 @@ -1881,11 +1924,11 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage { ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); } - auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - EXPECT_TRUE(ok_write); + auto [write_result, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_EQ(write_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_write.empty()); - if (test_for_changing_values) + if (test_for_changing_values && is_write_active) { auto status_map = rm->get_components_status(); auto check_periodicity = [&](const std::string & component_name, unsigned int rate) @@ -1949,7 +1992,7 @@ TEST_F( { setup_resource_manager_and_do_initial_checks(false); - check_read_and_write_cycles(true); + check_read_and_write_cycles(true, true); } TEST_F( @@ -1958,7 +2001,7 @@ TEST_F( { setup_resource_manager_and_do_initial_checks(true); - check_read_and_write_cycles(true); + check_read_and_write_cycles(true, true); } TEST_F( @@ -1986,7 +2029,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - check_read_and_write_cycles(true); + check_read_and_write_cycles(true, false); } TEST_F( @@ -2014,7 +2057,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - check_read_and_write_cycles(true); + check_read_and_write_cycles(true, false); } TEST_F( @@ -2042,7 +2085,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - check_read_and_write_cycles(false); + check_read_and_write_cycles(false, false); } TEST_F( @@ -2070,7 +2113,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - check_read_and_write_cycles(false); + check_read_and_write_cycles(false, false); } TEST_F( @@ -2098,7 +2141,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); - check_read_and_write_cycles(false); + check_read_and_write_cycles(false, false); } TEST_F( @@ -2126,7 +2169,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); - check_read_and_write_cycles(false); + check_read_and_write_cycles(false, false); } TEST_F( @@ -2139,14 +2182,14 @@ TEST_F( const auto read = [&]() { - const auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + const auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); }; const auto write = [&]() { - const auto [ok, failed_hardware_names] = rm->write(time, duration); - EXPECT_TRUE(ok); + const auto [result, failed_hardware_names] = rm->write(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); }; @@ -2266,15 +2309,15 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest 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); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_THAT(failed_hardware_names, testing::IsEmpty()); } { // 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); + auto [result, failed_hardware_names] = rm->write(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_THAT(failed_hardware_names, testing::IsEmpty()); } node_.get_clock()->sleep_until(time + duration); @@ -2303,7 +2346,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest } }; - void check_read_and_write_cycles(bool check_for_updated_values) + void check_read_and_write_cycles(bool check_for_updated_values, bool is_write_active) { double prev_act_state_value = state_itfs[0].get_optional().value(); double prev_system_state_value = state_itfs[1].get_optional().value(); @@ -2311,8 +2354,8 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest 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); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); // The values are computations exactly within the test_components if (check_for_updated_values) @@ -2320,16 +2363,18 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest prev_system_state_value = claimed_itfs[1].get_optional().value() / 2.0; prev_act_state_value = claimed_itfs[0].get_optional().value() / 2.0; } - claimed_itfs[0].set_value(claimed_itfs[0].get_optional().value() + actuator_increment); - claimed_itfs[1].set_value(claimed_itfs[1].get_optional().value() + system_increment); + ASSERT_TRUE( + claimed_itfs[0].set_value(claimed_itfs[0].get_optional().value() + actuator_increment)); + ASSERT_TRUE( + claimed_itfs[1].set_value(claimed_itfs[1].get_optional().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_optional().value(), prev_act_state_value, actuator_increment / 2.0); ASSERT_NEAR( state_itfs[1].get_optional().value(), prev_system_state_value, system_increment / 2.0); - auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - EXPECT_TRUE(ok_write); + auto [write_result, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_EQ(write_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_write.empty()); node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); @@ -2359,7 +2404,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); }; - if (check_for_updated_values) + if (check_for_updated_values && is_write_active) { const unsigned int rw_rate = 100u; const double expec_read_execution_time = (1.e6 / (3 * rw_rate)) + 200.0; @@ -2402,7 +2447,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_activate) { setup_resource_manager_and_do_initial_checks(); - check_read_and_write_cycles(true); + check_read_and_write_cycles(true, true); } TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_deactivate) @@ -2428,7 +2473,7 @@ TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_ status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - check_read_and_write_cycles(true); + check_read_and_write_cycles(true, false); } TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_unconfigured) @@ -2454,7 +2499,7 @@ TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_ status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - check_read_and_write_cycles(false); + check_read_and_write_cycles(false, false); } TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_finalized) @@ -2480,7 +2525,7 @@ TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_ status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); - check_read_and_write_cycles(false); + check_read_and_write_cycles(false, false); } class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest @@ -2518,15 +2563,15 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest 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); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::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); + ASSERT_TRUE(claimed_itfs[0].set_value(10.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(20.0)); + auto [result, failed_hardware_names] = rm->write(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); } @@ -2558,15 +2603,15 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest void check_limit_enforcement() { { - auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); - claimed_itfs[0].set_value(2.0); - claimed_itfs[1].set_value(-4.0); + ASSERT_TRUE(claimed_itfs[0].set_value(2.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(-4.0)); - auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - EXPECT_TRUE(ok_write); + auto [write_result, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_EQ(write_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_write.empty()); node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); @@ -2575,14 +2620,14 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest { // read now and check that without limit enforcement the values are half of command as this is // the logic implemented in test components - auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); - EXPECT_EQ(state_itfs[0].get_value(), 1.0); - EXPECT_EQ(state_itfs[1].get_value(), -2.0); - auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - EXPECT_TRUE(ok_write); + EXPECT_EQ(state_itfs[0].get_optional().value(), 1.0); + EXPECT_EQ(state_itfs[1].get_optional().value(), -2.0); + auto [write_result, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(write_result == hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_write.empty()); node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); @@ -2591,72 +2636,74 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest // Let's enforce for one loop and then run the read and write again and reset interfaces to zero // state { - auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); - EXPECT_EQ(state_itfs[0].get_value(), 1.0); - EXPECT_EQ(state_itfs[1].get_value(), -2.0); + EXPECT_EQ(state_itfs[0].get_optional().value(), 1.0); + EXPECT_EQ(state_itfs[1].get_optional().value(), -2.0); - EXPECT_EQ(claimed_itfs[0].get_value(), 2.0); - EXPECT_EQ(claimed_itfs[1].get_value(), -4.0); - claimed_itfs[0].set_value(0.0); - claimed_itfs[1].set_value(0.0); - EXPECT_EQ(claimed_itfs[0].get_value(), 0.0); - EXPECT_EQ(claimed_itfs[1].get_value(), 0.0); + EXPECT_EQ(claimed_itfs[0].get_optional().value(), 2.0); + EXPECT_EQ(claimed_itfs[1].get_optional().value(), -4.0); + ASSERT_TRUE(claimed_itfs[0].set_value(0.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(0.0)); + EXPECT_EQ(claimed_itfs[0].get_optional().value(), 0.0); + EXPECT_EQ(claimed_itfs[1].get_optional().value(), 0.0); // enforcing limits rm->enforce_command_limits(duration); - ASSERT_NEAR(state_itfs[2].get_value(), 1.05, 0.00001); + ASSERT_NEAR(state_itfs[2].get_optional().value(), 1.05, 0.00001); // It is using the actual velocity 1.05 to limit the claimed_itf - EXPECT_NEAR(claimed_itfs[0].get_value(), 1.048, 0.00001); - EXPECT_EQ(claimed_itfs[1].get_value(), 0.0); + EXPECT_NEAR(claimed_itfs[0].get_optional().value(), 1.048, 0.00001); + EXPECT_EQ(claimed_itfs[1].get_optional().value(), 0.0); - auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - EXPECT_TRUE(ok_write); + auto [write_result, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_EQ(write_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_write.empty()); node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); - auto [read_ok, failed_hardware_names_read] = rm->read(time, duration); - EXPECT_TRUE(read_ok); + auto [read_result, failed_hardware_names_read] = rm->read(time, duration); + EXPECT_EQ(read_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_read.empty()); - ASSERT_NEAR(state_itfs[0].get_value(), claimed_itfs[0].get_value() / 2.0, 0.00001); - ASSERT_EQ(state_itfs[1].get_value(), 0.0); + ASSERT_NEAR( + state_itfs[0].get_optional().value(), claimed_itfs[0].get_optional().value() / 2.0, + 0.00001); + ASSERT_EQ(state_itfs[1].get_optional().value(), 0.0); } // Reset the position state interface of actuator to zero { - ASSERT_GT(state_itfs[2].get_value(), 1.05); - claimed_itfs[0].set_value(test_constants::RESET_STATE_INTERFACES_VALUE); - auto [read_ok, failed_hardware_names_read] = rm->read(time, duration); - EXPECT_TRUE(read_ok); + ASSERT_GT(state_itfs[2].get_optional().value(), 1.05); + ASSERT_TRUE(claimed_itfs[0].set_value(test_constants::RESET_STATE_INTERFACES_VALUE)); + auto [read_result, failed_hardware_names_read] = rm->read(time, duration); + EXPECT_EQ(read_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_read.empty()); - ASSERT_EQ(state_itfs[2].get_value(), 0.0); - claimed_itfs[0].set_value(0.0); - claimed_itfs[1].set_value(0.0); - ASSERT_EQ(claimed_itfs[0].get_value(), 0.0); - ASSERT_EQ(claimed_itfs[1].get_value(), 0.0); + ASSERT_EQ(state_itfs[2].get_optional().value(), 0.0); + ASSERT_TRUE(claimed_itfs[0].set_value(0.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(0.0)); + ASSERT_EQ(claimed_itfs[0].get_optional().value(), 0.0); + ASSERT_EQ(claimed_itfs[1].get_optional().value(), 0.0); } - double new_state_value_1 = state_itfs[0].get_value(); - double new_state_value_2 = state_itfs[1].get_value(); + double new_state_value_1 = state_itfs[0].get_optional().value(); + double new_state_value_2 = state_itfs[1].get_optional().value(); // Now loop and see that the joint limits are being enforced progressively for (size_t i = 1; i < 300; i++) { - auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); - EXPECT_EQ(state_itfs[0].get_value(), new_state_value_1); - EXPECT_EQ(state_itfs[1].get_value(), new_state_value_2); + EXPECT_EQ(state_itfs[0].get_optional().value(), new_state_value_1); + EXPECT_EQ(state_itfs[1].get_optional().value(), new_state_value_2); - claimed_itfs[0].set_value(10.0); - claimed_itfs[1].set_value(-20.0); - EXPECT_EQ(claimed_itfs[0].get_value(), 10.0); - EXPECT_EQ(claimed_itfs[1].get_value(), -20.0); + ASSERT_TRUE(claimed_itfs[0].set_value(10.0)); + ASSERT_TRUE(claimed_itfs[1].set_value(-20.0)); + EXPECT_EQ(claimed_itfs[0].get_optional().value(), 10.0); + EXPECT_EQ(claimed_itfs[1].get_optional().value(), -20.0); // enforcing limits rm->enforce_command_limits(duration); @@ -2665,32 +2712,32 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest const double velocity_joint_1 = 0.2; const double prev_command_val = 1.048; ASSERT_NEAR( - claimed_itfs[0].get_value(), + claimed_itfs[0].get_optional().value(), prev_command_val + std::min((velocity_joint_1 * (duration.seconds() * static_cast(i))), M_PI), 1.0e-8) << "This should be progressively increasing as it is a position limit for iteration : " << i; - EXPECT_NEAR(claimed_itfs[1].get_value(), -0.2, 1.0e-8) + EXPECT_NEAR(claimed_itfs[1].get_optional().value(), -0.2, 1.0e-8) << "This should be -0.2 as it is velocity limit"; // This is as per the logic of the test components internally - new_state_value_1 = claimed_itfs[0].get_value() / 2.0; - new_state_value_2 = claimed_itfs[1].get_value() / 2.0; + new_state_value_1 = claimed_itfs[0].get_optional().value() / 2.0; + new_state_value_2 = claimed_itfs[1].get_optional().value() / 2.0; - auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - EXPECT_TRUE(ok_write); + auto [write_result, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_EQ(write_result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names_write.empty()); node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); } { - auto [ok, failed_hardware_names] = rm->read(time, duration); - EXPECT_TRUE(ok); + auto [result, failed_hardware_names] = rm->read(time, duration); + EXPECT_EQ(result, hardware_interface::return_type::OK); EXPECT_TRUE(failed_hardware_names.empty()); - EXPECT_NEAR(state_itfs[0].get_value(), 0.823, 0.00001); - EXPECT_NEAR(state_itfs[1].get_value(), -0.1, 0.00001); + EXPECT_NEAR(state_itfs[0].get_optional().value(), 0.823, 0.00001); + EXPECT_NEAR(state_itfs[1].get_optional().value(), -0.1, 0.00001); } } diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index c252c3a5be..64f171b81f 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -64,6 +64,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) { } + + explicit TestableResourceManager(const hardware_interface::ResourceManagerParams & params) + : hardware_interface::ResourceManager(params, true) + { + } }; std::vector set_components_state( diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index 48a8768744..810a4f8c85 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -14,6 +14,7 @@ // Authors: Dr. Denis +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" #include "test_resource_manager.hpp" #include @@ -104,7 +105,6 @@ class ResourceManagerPreparePerformTest : public ResourceManagerTest std::unique_ptr claimed_actuator_position_state_; // Scenarios defined by example criteria - rclcpp::Node node_{"ResourceManagerPreparePerformTest"}; std::vector empty_keys = {}; std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; std::vector legal_keys_system = {"joint1/position", "joint2/position"}; @@ -148,26 +148,33 @@ TEST_F( EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is UNCONFIGURED expect OK + // When TestActuatorHardware is UNCONFIGURED expect ERROR EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 403.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)) + << "The actuator HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 403.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 503.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)) + << "The actuator HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 503.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 603.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0) + << "Shouldn't change as the perform command mode switch is done only for actuator interfaces"; EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); }; @@ -183,46 +190,51 @@ TEST_F( // When TestSystemCommandModes is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is INACTIVE expect OK - EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 304.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 404.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + // When TestActuatorHardware is INACTIVE expect not OK + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7) + << "Start interfaces with inactive should result in no change"; + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7) + << "Start interfaces with inactive should result in no change"; - EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 405.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 505.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7) + << "Start interfaces with inactive should result in no change"; + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7) + << "Start interfaces with inactive should result in no change"; EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 506.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7); - EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 606.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)) + << "Inactive with empty start interfaces is OK"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); }; // System : INACTIVE @@ -234,56 +246,124 @@ TEST_F( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive", lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); - // When TestSystemCommandModes is INACTIVE expect OK - EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + // When TestSystemCommandModes is INACTIVE expect not OK + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0) + << "Start interfaces with inactive should result in no change"; + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0) + << "Start interfaces with inactive should result in no change"; + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0) + << "Start interfaces with inactive should result in no change"; + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0) + << "Start interfaces with inactive should result in no change"; + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); - EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); - EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); - EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); +}; + +// System : UNCONFIGURED +// Actuator: ACTIVE +TEST_F( + ResourceManagerPreparePerformTest, + when_system_unconfigured_and_actuator_active_expect_actuator_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is UNCONFIGURED expect error + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)) + << "The system HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); // When TestActuatorHardware is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 304.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 404.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 405.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 505.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 506.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7); - EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 606.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); }; // System : UNCONFIGURED -// Actuator: ACTIVE +// Actuator: DEACTIVATED then WRITE_IGNORED TEST_F( ResourceManagerPreparePerformTest, - when_system_unconfigured_and_actuator_active_expect_actuator_passing) + when_actuator_deactivated_then_write_error_is_ignored_and_remains_inactive) { preconfigure_components( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", @@ -292,46 +372,120 @@ TEST_F( // When TestSystemCommandModes is UNCONFIGURED expect error EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.100, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.100, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)) + << "The system HW component is unconfigured, so the perform should fail!"; EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.200, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.200, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.300, 1e-7); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is INACTIVE expect OK + // When TestActuatorHardware is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.301, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.401, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.402, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.502, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); + std::unique_ptr claimed_actuator_velocity_command_ = + std::make_unique( + rm_->claim_command_interface("joint3/position")); + + auto status_map = rm_->get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Now deactivate with write deactivate value + EXPECT_TRUE( + claimed_actuator_velocity_command_->set_value(test_constants::WRITE_DEACTIVATE_VALUE)); + rm_->write(node_.now(), rclcpp::Duration(0, 1000000)); + + status_map = rm_->get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // Similar to deactivate callback from write cycle EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.503, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.603, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + + // Similar to the proximal activation + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)) + << "Start interfaces with inactive should result in no change"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7) + << "Start interfaces with inactive should result in no change"; + + // Now return ERROR with write fail value + EXPECT_TRUE(claimed_actuator_velocity_command_->set_value(test_constants::WRITE_FAIL_VALUE)); + rm_->write(node_.now(), rclcpp::Duration(0, 1000000)); + + status_map = rm_->get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + // The component is INACTIVE, write does nothing, so no error is triggered. State remains + // INACTIVE. + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); }; // System : UNCONFIGURED @@ -348,45 +502,349 @@ TEST_F( EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - // When TestActuatorHardware is INACTIVE expect OK + // When TestActuatorHardware is FINALIZED expect ERROR EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); - EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); +}; + +class ResourceManagerControlWithInactiveHWPreparePerformTest : public ResourceManagerTest +{ +public: + void SetUp() + { + ResourceManagerTest::SetUp(); + + hardware_interface::ResourceManagerParams params; + params.allow_controller_activation_with_inactive_hardware = true; + params.logger = node_.get_logger(); + params.clock = node_.get_clock(); + params.robot_description = command_mode_urdf; + rm_ = std::make_unique(params); + ASSERT_EQ(1u, rm_->actuator_components_size()); + ASSERT_EQ(1u, rm_->system_components_size()); + + // empty call can be done at any time and it doesn't propagates to HW components - always true + ASSERT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + // empty call can be done at any time and it doesn't propagates to HW components - always true + ASSERT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + + // Set both HW to ACTIVE to claim interfaces. There should stay persistent because we are not + // cleaning them for now, so this is a good way to keep the access and "f* the system" + set_components_state( + *rm_, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + set_components_state( + *rm_, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + // State to get feedback how many times "prepare_for_switch" is called + claimed_system_acceleration_state_ = std::make_unique( + rm_->claim_state_interface("joint1/acceleration")); + claimed_actuator_position_state_ = std::make_unique( + rm_->claim_state_interface("joint3/position")); + } + + void preconfigure_components( + const uint8_t system_state_id, const std::string syste_state_name, + const uint8_t actuator_state_id, const std::string actuator_state_name) + { + set_components_state(*rm_, {"TestSystemCommandModes"}, system_state_id, syste_state_name); + set_components_state(*rm_, {"TestActuatorHardware"}, actuator_state_id, actuator_state_name); + + auto status_map = rm_->get_components_status(); + EXPECT_EQ(status_map["TestSystemCommandModes"].state.id(), system_state_id); + EXPECT_EQ(status_map["TestActuatorHardware"].state.id(), actuator_state_id); + } + + std::unique_ptr rm_; + + std::unique_ptr claimed_system_acceleration_state_; + std::unique_ptr claimed_actuator_position_state_; + + // Scenarios defined by example criteria + std::vector empty_keys = {}; + std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; + std::vector legal_keys_system = {"joint1/position", "joint2/position"}; + std::vector legal_keys_actuator = {"joint3/position"}; +}; + +// System : ACTIVE +// Actuator: INACTIVE +TEST_F( + ResourceManagerControlWithInactiveHWPreparePerformTest, + when_system_active_and_actuator_inactive_expect_both_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active", + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)) + << "Inactive is OK"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); +}; + +// System : INACTIVE +// Actuator: ACTIVE +TEST_F( + ResourceManagerControlWithInactiveHWPreparePerformTest, + when_system_inactive_and_actuator_active_expect_both_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0) + << "System interfaces shouldn't affect the actuator"; + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); +}; + +// System : UNCONFIGURED +// Actuator: DEACTIVATED then WRITE_IGNORED +TEST_F( + ResourceManagerControlWithInactiveHWPreparePerformTest, + when_actuator_deactivated_then_write_error_is_ignored_and_remains_inactive) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is UNCONFIGURED expect error + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)) + << "The system HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)) + << "The system HW component is unconfigured, so the perform should fail!"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7); + + std::unique_ptr claimed_actuator_velocity_command_ = + std::make_unique( + rm_->claim_command_interface("joint3/position")); + + auto status_map = rm_->get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Now deactivate with write deactivate value + EXPECT_TRUE( + claimed_actuator_velocity_command_->set_value(test_constants::WRITE_DEACTIVATE_VALUE)); + rm_->write(node_.now(), rclcpp::Duration(0, 1000000)); + + status_map = rm_->get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // Similar to deactivate callback from write cycle + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + + // Similar to the proximal activation + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)) + << "Start interfaces with inactive should result in no change"; + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7) + << "Start interfaces with inactive should result in no change"; + + // Now return ERROR with write fail value + EXPECT_TRUE(claimed_actuator_velocity_command_->set_value(test_constants::WRITE_FAIL_VALUE)); + rm_->write(node_.now(), rclcpp::Duration(0, 1000000)); + + status_map = rm_->get_components_status(); + EXPECT_EQ( + status_map["TestSystemCommandModes"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + // The component is INACTIVE, write does nothing, so no error is triggered. State remains + // INACTIVE. + EXPECT_EQ( + status_map["TestActuatorHardware"].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.607, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.707, 1e-7); }; int main(int argc, char ** argv) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 981896d9d4..8f3afbf507 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ + +5.7.0 (2025-10-03) +------------------ + +5.6.0 (2025-08-26) +------------------ +* Remove extra semicolons (`#2478 `_) +* Contributors: Tapia Danish + +5.5.0 (2025-07-31) +------------------ + +5.4.0 (2025-07-21) +------------------ +* Fix the crashing joint limiters when used with multiple interfaces (`#2371 `_) +* Contributors: Sai Kishor Kothakota + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ +* Use target_link_libraries instead of ament_target_dependencies (`#2266 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* Statically allocate string concatenations using FMT formatting (`#2205 `_) +* Contributors: mini-1235 + 4.29.0 (2025-05-04) ------------------- diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 762d9a0ee8..9a0937c581 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle trajectory_msgs urdf + fmt ) find_package(ament_cmake REQUIRED) @@ -27,7 +28,12 @@ target_include_directories(joint_limits INTERFACE $ $ ) -ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(joint_limits INTERFACE + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + urdf::urdf + realtime_tools::realtime_tools + fmt::fmt) add_library(joint_limiter_interface SHARED src/joint_limiter_interface.cpp @@ -37,7 +43,10 @@ target_include_directories(joint_limiter_interface PUBLIC $ $ ) -ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(joint_limiter_interface PUBLIC + joint_limits + pluginlib::pluginlib + ${trajectory_msgs_TARGETS}) add_library(joint_limits_helpers SHARED src/joint_limits_helpers.cpp @@ -47,7 +56,7 @@ target_include_directories(joint_limits_helpers PUBLIC $ $ ) -ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(joint_limits_helpers PUBLIC joint_limits) add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp @@ -59,9 +68,9 @@ target_include_directories(joint_saturation_limiter PUBLIC $ $ ) -target_link_libraries(joint_saturation_limiter PUBLIC joint_limits_helpers) - -ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(joint_saturation_limiter PUBLIC + joint_limits_helpers + joint_limiter_interface) pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) @@ -73,7 +82,7 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) ament_add_gmock(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_link_libraries(joint_limits_rosparam_test joint_limits) + target_link_libraries(joint_limits_rosparam_test joint_limits fmt::fmt) target_compile_definitions( joint_limits_rosparam_test PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/") @@ -95,30 +104,24 @@ if(BUILD_TESTING) ${CMAKE_CURRENT_SOURCE_DIR}/test/joint_saturation_limiter_param.yaml ) target_include_directories(test_joint_saturation_limiter PRIVATE include) - target_link_libraries(test_joint_saturation_limiter joint_limiter_interface) - ament_target_dependencies( - test_joint_saturation_limiter - pluginlib - rclcpp - ) + target_link_libraries(test_joint_saturation_limiter + joint_limiter_interface + pluginlib::pluginlib + rclcpp::rclcpp) ament_add_gmock(test_joint_range_limiter test/test_joint_range_limiter.cpp) target_include_directories(test_joint_range_limiter PRIVATE include) - target_link_libraries(test_joint_range_limiter joint_limiter_interface) - ament_target_dependencies( - test_joint_range_limiter - pluginlib - rclcpp - ) + target_link_libraries(test_joint_range_limiter + joint_limiter_interface + pluginlib::pluginlib + rclcpp::rclcpp) ament_add_gmock(test_joint_soft_limiter test/test_joint_soft_limiter.cpp) target_include_directories(test_joint_soft_limiter PRIVATE include) - target_link_libraries(test_joint_soft_limiter joint_limiter_interface) - ament_target_dependencies( - test_joint_soft_limiter - pluginlib - rclcpp - ) + target_link_libraries(test_joint_soft_limiter + joint_limiter_interface + pluginlib::pluginlib + rclcpp::rclcpp) endif() diff --git a/joint_limits/include/joint_limits/data_structures.hpp b/joint_limits/include/joint_limits/data_structures.hpp index 2eb782b6da..763247fa9d 100644 --- a/joint_limits/include/joint_limits/data_structures.hpp +++ b/joint_limits/include/joint_limits/data_structures.hpp @@ -17,6 +17,8 @@ #ifndef JOINT_LIMITS__DATA_STRUCTURES_HPP_ #define JOINT_LIMITS__DATA_STRUCTURES_HPP_ +#include + #include #include #include @@ -36,10 +38,10 @@ namespace joint_limits { -DEFINE_LIMIT_STRUCT(PositionLimits); -DEFINE_LIMIT_STRUCT(VelocityLimits); -DEFINE_LIMIT_STRUCT(EffortLimits); -DEFINE_LIMIT_STRUCT(AccelerationLimits); +DEFINE_LIMIT_STRUCT(PositionLimits) +DEFINE_LIMIT_STRUCT(VelocityLimits) +DEFINE_LIMIT_STRUCT(EffortLimits) +DEFINE_LIMIT_STRUCT(AccelerationLimits) struct JointControlInterfacesData { @@ -68,6 +70,7 @@ struct JointControlInterfacesData std::string to_string() const { std::string str; + str += "Joint: '" + joint_name + "', "; if (has_position()) { str += "position: " + std::to_string(position.value()) + ", "; @@ -115,10 +118,20 @@ struct JointInterfacesCommandLimiterData bool has_limited_data() const { return limited.has_data(); } + void set_joint_name(const std::string & name) + { + joint_name = name; + actual.joint_name = name; + command.joint_name = name; + limited.joint_name = name; + prev_command.joint_name = name; + } + std::string to_string() const { - return "Joint : '" + joint_name + "', (actual: [" + actual.to_string() + "], command : [" + - command.to_string() + "], limited: [" + limited.to_string() + "])"; + return fmt::format( + FMT_COMPILE("Joint: '{}', (actual: [{}], command: [{}], limited: [{}])"), joint_name, + actual.to_string(), command.to_string(), limited.to_string()); } }; diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 3af2d313ad..b879d4e316 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -32,6 +32,14 @@ constexpr double POSITION_BOUNDS_TOLERANCE = 0.002; constexpr double OUT_OF_BOUNDS_EXCEPTION_TOLERANCE = 0.0087; } // namespace internal +/** + * @brief Updates the previous command with the desired command. + * @param prev_command The previous command to update. + * @param desired The desired command which is limited. + */ +void update_prev_command( + const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command); + /** * @brief Checks if a value is limited by the given limits. * @param value The value to check. diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 379cee0357..b06a1e5463 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -17,6 +17,8 @@ #ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ #define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#include + #include #include #include @@ -88,7 +90,7 @@ inline bool declare_parameters( const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf) { - const std::string param_base_name = "joint_limits." + joint_name; + const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name); try { auto_declare(param_itf, param_base_name + ".has_position_limits", false); @@ -228,7 +230,7 @@ inline bool get_joint_limits( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, JointLimits & limits) { - const std::string param_base_name = "joint_limits." + joint_name; + const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name); try { if ( @@ -433,7 +435,7 @@ inline bool check_for_limits_update( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, JointLimits & updated_limits) { - const std::string param_base_name = "joint_limits." + joint_name; + const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name); bool changed = false; // update first numerical values to make later checks for "has" limits members @@ -664,7 +666,7 @@ inline bool get_joint_limits( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, SoftJointLimits & soft_limits) { - const std::string param_base_name = "joint_limits." + joint_name; + const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name); try { if ( @@ -774,7 +776,7 @@ inline bool check_for_limits_update( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, SoftJointLimits & updated_limits) { - const std::string param_base_name = "joint_limits." + joint_name; + const std::string param_base_name = fmt::format(FMT_COMPILE("joint_limits.{}"), joint_name); bool changed = false; for (auto & parameter : parameters) diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 3e4bc1eac9..3a4ba2e47f 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -41,7 +41,7 @@ class JointSaturationLimiter : public JointLimiterInterface { public: + virtual ~JointSoftLimiter() = default; + bool on_init() override { const bool result = (number_of_joints_ == 1); diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 6e091bbbe1..e76329efae 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.29.0 + 6.0.1 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 @@ -25,6 +25,7 @@ rclcpp_lifecycle trajectory_msgs urdf + fmt ament_cmake_gmock generate_parameter_library diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 27a156c584..5a49c7ba88 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -15,8 +15,12 @@ /// \author Adrià Roig Moreno #include "joint_limits/joint_limits_helpers.hpp" + +#include + #include #include + #include "rclcpp/logging.hpp" namespace joint_limits @@ -53,13 +57,13 @@ void verify_actual_position_within_limits( actual_pos > (limits.max_position + internal::OUT_OF_BOUNDS_EXCEPTION_TOLERANCE) || actual_pos < (limits.min_position - internal::OUT_OF_BOUNDS_EXCEPTION_TOLERANCE)) { - const std::string error_message = - "Joint position is out of bounds for the joint : '" + joint_name + - "' actual position: " + std::to_string(actual_pos) + " limits: [" + - std::to_string(limits.min_position) + ", " + std::to_string(limits.max_position) + - "]. This could be due to a hardware failure (or) the physical limits of the joint being " - "larger than the ones defined in the URDF. Please recheck the URDF and the hardware to " - "verify the joint limits."; + const std::string error_message = fmt::format( + FMT_COMPILE( + "Joint position is out of bounds for the joint : '{}' actual position: {} limits: [{}, " + "{}]. This could be due to a hardware failure (or) the physical limits of the joint " + "being larger than the ones defined in the URDF. Please recheck the URDF and the " + "hardware to verify the joint limits."), + joint_name, actual_pos, limits.min_position, limits.max_position); RCLCPP_ERROR_ONCE(rclcpp::get_logger("joint_limiter_interface"), "%s", error_message.c_str()); // Throw an exception to indicate that the joint position is out of bounds throw std::runtime_error(error_message); @@ -68,6 +72,32 @@ void verify_actual_position_within_limits( } } // namespace internal +void update_prev_command( + const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command) +{ + if (desired.has_position()) + { + prev_command.position = desired.position; + } + if (desired.has_velocity()) + { + prev_command.velocity = desired.velocity; + } + if (desired.has_effort()) + { + prev_command.effort = desired.effort; + } + if (desired.has_acceleration()) + { + prev_command.acceleration = desired.acceleration; + } + if (desired.has_jerk()) + { + prev_command.jerk = desired.jerk; + } + prev_command.joint_name = desired.joint_name; +} + bool is_limited(double value, double min, double max) { return value < min || value > max; } PositionLimits compute_position_limits( diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 2baa848cbc..b5e13c538c 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -61,45 +61,26 @@ bool JointSaturationLimiter::on_enforce( // The following conditional filling is needed for cases of having certain information missing if (!prev_command_.has_data()) { - if (actual.has_position()) + if (desired.has_position()) { - prev_command_.position = actual.position; + prev_command_.position = actual.has_position() ? actual.position : desired.position; } - else if (desired.has_position()) + if (desired.has_velocity()) { - prev_command_.position = desired.position; + prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity; } - if (actual.has_velocity()) + if (desired.has_effort()) { - prev_command_.velocity = actual.velocity; + prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort; } - else if (desired.has_velocity()) + if (desired.has_acceleration()) { - prev_command_.velocity = desired.velocity; + prev_command_.acceleration = + actual.has_acceleration() ? actual.acceleration : desired.acceleration; } - if (actual.has_effort()) + if (desired.has_jerk()) { - prev_command_.effort = actual.effort; - } - else if (desired.has_effort()) - { - prev_command_.effort = desired.effort; - } - if (actual.has_acceleration()) - { - prev_command_.acceleration = actual.acceleration; - } - else if (desired.has_acceleration()) - { - prev_command_.acceleration = desired.acceleration; - } - if (actual.has_jerk()) - { - prev_command_.jerk = actual.jerk; - } - else if (desired.has_jerk()) - { - prev_command_.jerk = desired.jerk; + prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk; } if (actual.has_data()) { @@ -159,7 +140,7 @@ bool JointSaturationLimiter::on_enforce( desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); } - prev_command_ = desired; + update_prev_command(desired, prev_command_); return limits_enforced; } diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index 07f943dfa8..ff86a00765 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -44,45 +44,26 @@ bool JointSoftLimiter::on_enforce( if (!prev_command_.has_data()) { - if (actual.has_position()) + if (desired.has_position()) { - prev_command_.position = actual.position; + prev_command_.position = actual.has_position() ? actual.position : desired.position; } - else if (desired.has_position()) + if (desired.has_velocity()) { - prev_command_.position = desired.position; + prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity; } - if (actual.has_velocity()) + if (desired.has_effort()) { - prev_command_.velocity = actual.velocity; + prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort; } - else if (desired.has_velocity()) + if (desired.has_acceleration()) { - prev_command_.velocity = desired.velocity; + prev_command_.acceleration = + actual.has_acceleration() ? actual.acceleration : desired.acceleration; } - if (actual.has_effort()) + if (desired.has_jerk()) { - prev_command_.effort = actual.effort; - } - else if (desired.has_effort()) - { - prev_command_.effort = desired.effort; - } - if (actual.has_acceleration()) - { - prev_command_.acceleration = actual.acceleration; - } - else if (desired.has_acceleration()) - { - prev_command_.acceleration = desired.acceleration; - } - if (actual.has_jerk()) - { - prev_command_.jerk = actual.jerk; - } - else if (desired.has_jerk()) - { - prev_command_.jerk = desired.jerk; + prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk; } if (actual.has_data()) { @@ -266,7 +247,7 @@ bool JointSoftLimiter::on_enforce( limits_enforced = true; } - prev_command_ = desired; + update_prev_command(desired, prev_command_); return limits_enforced; } diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index 7b0227602e..8b8d865810 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -548,6 +548,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) limits.max_effort = 200.0; joint_limits::SoftJointLimits soft_limits; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); // Reset the desired and actual states @@ -651,6 +652,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) // Check with high values of k_velocity (hard limits should be considered) soft_limits.k_velocity = 5000.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : @@ -674,6 +676,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) // Check with low values of k_velocity soft_limits.k_velocity = 300.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : @@ -702,6 +705,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) soft_limits.min_position = -4.0; soft_limits.max_position = 4.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); test_limit_enforcing(0.0, 0.5, 20.0, 20.0, false); @@ -762,6 +766,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) soft_limits.min_position = -10.0; soft_limits.max_position = 10.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : @@ -788,6 +793,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) soft_limits.min_position = -10.0; soft_limits.max_position = 10.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : diff --git a/ros2_control-not-released.kilted.repos b/ros2_control-not-released.kilted.repos new file mode 100644 index 0000000000..56f46b6f79 --- /dev/null +++ b/ros2_control-not-released.kilted.repos @@ -0,0 +1 @@ +repositories: diff --git a/ros2_control.humble.repos b/ros2_control.humble.repos index af5cbc3c26..6611229cd2 100644 --- a/ros2_control.humble.repos +++ b/ros2_control.humble.repos @@ -1,9 +1,9 @@ repositories: - ros-controls/realtime_tools: + realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: humble - ros-controls/control_msgs: + control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git version: humble diff --git a/ros2_control.jazzy.repos b/ros2_control.jazzy.repos index 7b4883be41..395a500a4c 100644 --- a/ros2_control.jazzy.repos +++ b/ros2_control.jazzy.repos @@ -1,13 +1,13 @@ repositories: - ros-controls/realtime_tools: + realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: jazzy - ros-controls/control_msgs: + control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git version: jazzy - ros-controls/ros2_control_cmake: + ros2_control_cmake: type: git url: https://github.com/ros-controls/ros2_control_cmake.git version: master diff --git a/ros2_control.kilted.repos b/ros2_control.kilted.repos new file mode 100644 index 0000000000..9051805d9b --- /dev/null +++ b/ros2_control.kilted.repos @@ -0,0 +1,13 @@ +repositories: + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: kilted + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: master + ros2_control_cmake: + type: git + url: https://github.com/ros-controls/ros2_control_cmake.git + version: master diff --git a/ros2_control.rolling.repos b/ros2_control.rolling.repos index 9c65e8f84e..898c721529 100644 --- a/ros2_control.rolling.repos +++ b/ros2_control.rolling.repos @@ -1,13 +1,13 @@ repositories: - ros-controls/realtime_tools: + realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master - ros-controls/control_msgs: + control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git version: master - ros-controls/ros2_control_cmake: + ros2_control_cmake: type: git url: https://github.com/ros-controls/ros2_control_cmake.git version: master diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index f70fb21f57..a92e206180 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ + +5.7.0 (2025-10-03) +------------------ + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ + +5.4.0 (2025-07-21) +------------------ + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ + +5.0.0 (2025-05-21) +------------------ + 4.29.0 (2025-05-04) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 09d3a908e0..c8d4343663 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.29.0 + 6.0.1 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 64c7518eb8..54e644e1b9 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ + +5.7.0 (2025-10-03) +------------------ +* Add detach async policy for rate critical frameworks (`#2477 `_) +* Contributors: Sai Kishor Kothakota + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ + +5.4.0 (2025-07-21) +------------------ + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ +* Read `data_type` for all types of interfaces (`#2235 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ + 4.29.0 (2025-05-04) ------------------- * [Diagnostics] Add diagnostics of execution time and periodicity of the hardware components (`#2086 `_) 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 46feee39a9..45140fee1e 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 @@ -735,6 +735,32 @@ const auto valid_urdf_ros2_control_parameter_empty = )"; +const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type_on_joint_sensor_and_gpio = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType + 2 + 2 + + + + + + + + + + + + + + + + + +)"; + // Errors const auto invalid_urdf_ros2_control_invalid_child = R"( 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 b92957c1c6..b81a0ca985 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 @@ -698,6 +698,9 @@ const auto hardware_resources = const auto async_hardware_resources = R"( + + + test_actuator @@ -718,7 +721,10 @@ const auto async_hardware_resources = - + + + + test_system 2 @@ -2165,6 +2171,54 @@ const auto diff_drive_robot_sdf = )"; +const auto invalid_urdf_without_hardware_plugin = + R"( + + + + + + + + + + + + + +)"; + +const auto invalid_urdf_with_wrong_plugin = + R"( + + + + + + + + + + + + + mock_components/NonExistentSystem + + + +)"; + +const auto invalid_urdf_no_geometry = +R"( + + + + mock_components/NonExistentSystem + + + +)"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) + diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 95d981565c..9e9a3be8ac 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.29.0 + 6.0.1 Shared test resources for ros2_control stack Bence Magyar Denis Štogl diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 9528944ba5..1b47bb6af0 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ +* Added `view_hardware_status` ros2cli verb (`#2495 `_) +* Contributors: Soham Patil + +5.7.0 (2025-10-03) +------------------ + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ + +5.4.0 (2025-07-21) +------------------ +* Fix setuptools deprecations (`#2395 `_) +* Contributors: mosfet80 + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ +* Cleanup deprecations in `ros_control` (`#2258 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* [CM] Set default strictness of switch_controllers using parameters (`#2168 `_) +* Add `data_type` field to the HardwareInterfaces message (`#2204 `_) +* Contributors: Sai Kishor Kothakota + 4.29.0 (2025-05-04) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 8449583d61..d550dda158 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -20,6 +20,7 @@ Currently supported commands are - ros2 control switch_controllers - ros2 control unload_controller - ros2 control view_controller_chains + - ros2 control view_hardware_status list_controllers @@ -28,8 +29,8 @@ list_controllers .. code-block:: console $ ros2 control list_controllers -h - usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-s] [--claimed-interfaces] [--required-state-interfaces] [--required-command-interfaces] [--chained-interfaces] [--reference-interfaces] [--verbose] - [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-s] [--claimed-interfaces] [--required-state-interfaces] [--required-command-interfaces] [--chained-interfaces] [--exported-interfaces] [--verbose] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + [--ros-args ...] Output the list of loaded controllers, their type and status @@ -44,8 +45,8 @@ list_controllers --required-command-interfaces List controller's required command interfaces --chained-interfaces List interfaces that the controllers are chained to - --reference-interfaces - List controller's exported references + --exported-interfaces + List controller's exported state and reference interfaces --verbose, -v List controller's claimed interfaces, required state interfaces and required command interfaces -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node (default: controller_manager) @@ -107,7 +108,7 @@ list_hardware_components --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time - --verbose, -v List hardware components with command and state interfaces + --verbose, -v List hardware components with command and state interfaces along with their data types -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes @@ -124,6 +125,25 @@ Example output: type: system plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware state: id=3 label=active + command interfaces + joint2/position [available] [claimed] + joint1/position [available] [claimed] + + +.. code-block:: console + + $ ros2 control list_hardware_components -v + Hardware Component 0 + name: RRBot + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + joint2/position [double] [available] [claimed] + joint1/position [double] [available] [claimed] + state interfaces + joint2/position [double] [available] + joint1/position [double] [available] list_hardware_interfaces @@ -132,7 +152,7 @@ list_hardware_interfaces .. code-block:: console $ ros2 control list_hardware_interfaces -h - usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] + usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-s] [--verbose] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of available command and state interfaces @@ -141,6 +161,7 @@ list_hardware_interfaces --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time + --verbose, -v List hardware interfaces and their data types -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes @@ -158,6 +179,16 @@ list_hardware_interfaces joint1/position joint2/position +.. code-block:: console + + $ ros2 control list_hardware_interfaces -v + command interfaces + joint1/position [double] [unclaimed] + joint2/position [double] [unclaimed] + state interfaces + joint1/position [double] + joint2/position [double] + load_controller --------------- @@ -334,3 +365,24 @@ view_controller_chains --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable + + +view_hardware_status +---------------------- + +.. code-block:: console + + $ ros2 control view_hardware_status -h + usage: ros2 control view_hardware_status [-h] [--spin-time SPIN_TIME] [-s] [-i HARDWARE_ID] [-d DEVICE_ID] + + Echo hardware status messages with filtering capabilities + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -i HARDWARE_ID, --hardware-id HARDWARE_ID + Filter by a specific hardware component ID. + -d DEVICE_ID, --device-id DEVICE_ID + Filter by a specific device ID within a hardware component. diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index e4954a300d..16f10c6949 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.29.0 + 6.0.1 The ROS 2 command line tools for ros2_control. Bence Magyar Denis Štogl @@ -18,6 +18,7 @@ ros2param controller_manager controller_manager_msgs + control_msgs rosidl_runtime_py python3-pygraphviz diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 306e6f3af0..27c6bdb536 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import warnings from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments @@ -54,11 +53,11 @@ def print_controller_state(c, args, col_width_name, col_width_state, col_width_t for connection in c.chain_connections: for reference in connection.reference_interfaces: print(f"\t\t{reference:20s}") - if args.reference_interfaces or args.exported_interfaces or args.verbose: + if args.exported_interfaces or args.verbose: print("\texported reference interfaces:") for reference_interface in c.reference_interfaces: print(f"\t\t{reference_interface}") - if args.reference_interfaces or args.exported_interfaces or args.verbose: + if args.exported_interfaces or args.verbose: print("\texported state interfaces:") for exported_state_interface in c.exported_state_interfaces: print(f"\t\t{exported_state_interface}") @@ -89,11 +88,6 @@ def add_arguments(self, parser, cli_name): action="store_true", help="List interfaces that the controllers are chained to", ) - parser.add_argument( - "--reference-interfaces", - action="store_true", - help="List controller's exported references", - ) parser.add_argument( "--exported-interfaces", action="store_true", @@ -111,13 +105,6 @@ def main(self, *, args): with NodeStrategy(args).direct_node as node: response = list_controllers(node, args.controller_manager) - if args.reference_interfaces: - warnings.filterwarnings("always") - warnings.warn( - "The '--reference-interfaces' argument is deprecated and will be removed in future releases. Use '--exported-interfaces' instead.", - DeprecationWarning, - ) - if not response.controller: print("No controllers are currently loaded!") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index c7af8f5d60..bcf7778763 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -31,7 +31,7 @@ def add_arguments(self, parser, cli_name): "--verbose", "-v", action="store_true", - help="List hardware components with command and state interfaces", + help="List hardware components with command and state interfaces along with their data types", ) add_controller_mgr_parsers(parser) @@ -64,6 +64,7 @@ def main(self, *, args): f"\tis_async: {component.is_async}\n" f"\tcommand interfaces" ) + data_type_str = "" for cmd_interface in component.command_interfaces: if cmd_interface.is_available: available_str = f"{bcolors.OKBLUE}[available]{bcolors.ENDC}" @@ -75,7 +76,10 @@ def main(self, *, args): else: claimed_str = "[unclaimed]" - print(f"\t\t{cmd_interface.name} {available_str} {claimed_str}") + if args.verbose: + data_type_str = f" [{cmd_interface.data_type}]" + + print(f"\t\t{cmd_interface.name}{data_type_str} {available_str} {claimed_str}") if args.verbose: print("\tstate interfaces") @@ -85,6 +89,8 @@ def main(self, *, args): else: available_str = f"{bcolors.WARNING}[unavailable]{bcolors.ENDC}" - print(f"\t\t{state_interface.name} {available_str}") + print( + f"\t\t{state_interface.name} [{state_interface.data_type}] {available_str}" + ) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index a0bf8a8403..b854852c29 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -25,6 +25,12 @@ class ListHardwareInterfacesVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) + parser.add_argument( + "--verbose", + "-v", + action="store_true", + help="List hardware interfaces and their data types", + ) add_controller_mgr_parsers(parser) def main(self, *, args): @@ -37,26 +43,32 @@ def main(self, *, args): hardware_interfaces.state_interfaces, key=lambda hwi: hwi.name ) print("command interfaces") + data_type_str = "" for command_interface in command_interfaces: + if args.verbose: + data_type_str = f" [{command_interface.data_type}]" if command_interface.is_available: if command_interface.is_claimed: print( - f"\t{bcolors.OKBLUE}{command_interface.name} " - f"[available] [claimed]{bcolors.ENDC}" + f"\t{bcolors.OKBLUE}{command_interface.name}{data_type_str}" + f" [available] [claimed]{bcolors.ENDC}" ) else: print( - f"\t{bcolors.OKCYAN}{command_interface.name} " - f"[available] [unclaimed]{bcolors.ENDC}" + f"\t{bcolors.OKCYAN}{command_interface.name}{data_type_str}" + f" [available] [unclaimed]{bcolors.ENDC}" ) else: print( - f"\t{bcolors.WARNING}{command_interface.name} " - f"[unavailable] [unclaimed]{bcolors.ENDC}" + f"\t{bcolors.WARNING}{command_interface.name}{data_type_str}" + f" [unavailable] [unclaimed]{bcolors.ENDC}" ) print("state interfaces") + data_type_str = "" for state_interface in state_interfaces: - print(f"\t{state_interface.name}") + if args.verbose: + data_type_str = f" [{state_interface.data_type}]" + print(f"\t{state_interface.name}{data_type_str}") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 0d4d742038..0e90925d59 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -13,6 +13,7 @@ # limitations under the License. from controller_manager import switch_controllers, bcolors +from controller_manager_msgs.srv import SwitchController from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -40,7 +41,17 @@ def add_arguments(self, parser, cli_name): help="Name of the controllers to be activated", ) arg.completer = LoadedControllerNameCompleter(["inactive"]) - parser.add_argument("--strict", action="store_true", help="Strict switch") + strictness_group = parser.add_mutually_exclusive_group(required=False) + strictness_group.add_argument( + "--strict", + help="Set the switch_controllers service strictness to strict", + action="store_true", + ) + strictness_group.add_argument( + "--best-effort", + help="Set the switch_controllers service strictness to best effort", + action="store_true", + ) parser.add_argument("--activate-asap", action="store_true", help="Start asap controllers") parser.add_argument( "--switch-timeout", @@ -53,12 +64,17 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args).direct_node as node: + strictness = 0 + if args.strict: + strictness = SwitchController.Request.STRICT + elif args.best_effort: + strictness = SwitchController.Request.BEST_EFFORT response = switch_controllers( node, args.controller_manager, args.deactivate, args.activate, - args.strict, + strictness, args.activate_asap, args.switch_timeout, ) diff --git a/ros2controlcli/ros2controlcli/verb/view_hardware_status.py b/ros2controlcli/ros2controlcli/verb/view_hardware_status.py new file mode 100644 index 0000000000..4454c3fe7d --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/view_hardware_status.py @@ -0,0 +1,177 @@ +# Copyright 2025 ROS-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. + +from functools import partial +import datetime + +import rclpy +from ros2cli.node.direct import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2cli.verb import VerbExtension +from ros2topic.api import get_topic_names_and_types + +from control_msgs.msg import HardwareStatus +from controller_manager import bcolors + +from rosidl_runtime_py import message_to_yaml + +_DISCOVERY_THRESHOLD = 10 + + +class ViewHardwareStatusVerb(VerbExtension): + """Echo hardware status messages with filtering capabilities.""" + + def __init__(self): + super().__init__() + self.found_hardware_ids = set() + self.found_device_ids = set() + self.message_count = 0 + self.discovery_complete = False + + def add_arguments(self, parser, cli_name): + add_arguments(parser) + parser.add_argument( + "-i", + "--hardware-id", + dest="hardware_id", + help="Filter by a specific hardware component ID.", + ) + parser.add_argument( + "-d", + "--device-id", + dest="device_id", + help="Filter by a specific device ID within a hardware component.", + ) + + def _on_message(self, msg, args): + self.message_count += 1 + self.found_hardware_ids.add(msg.hardware_id) + for device_state in msg.hardware_device_states: + self.found_device_ids.add(device_state.device_id) + + if not self.discovery_complete and self.message_count >= _DISCOVERY_THRESHOLD: + self.discovery_complete = True + + if args.hardware_id and args.hardware_id not in self.found_hardware_ids: + print( + f"\n{bcolors.FAIL}Error: Hardware ID '{args.hardware_id}' not found.{bcolors.ENDC}" + ) + if self.found_hardware_ids: + print(f"{bcolors.OKBLUE}Available Hardware IDs:{bcolors.ENDC}") + for hw_id in sorted(self.found_hardware_ids): + print(f"\t{hw_id}") + else: + print(f"{bcolors.WARNING}No hardware IDs discovered.{bcolors.ENDC}") + rclpy.shutdown() + return + + if args.device_id and args.device_id not in self.found_device_ids: + print( + f"\n{bcolors.FAIL}Error: Device ID '{args.device_id}' not found.{bcolors.ENDC}" + ) + if self.found_device_ids: + print(f"{bcolors.OKBLUE}Available Device IDs:{bcolors.ENDC}") + for dev_id in sorted(self.found_device_ids): + print(f"\t{dev_id}") + else: + print(f"{bcolors.WARNING}No device IDs discovered.{bcolors.ENDC}") + rclpy.shutdown() + return + + if args.hardware_id and msg.hardware_id != args.hardware_id: + return + + if args.device_id and not any( + d.device_id == args.device_id for d in msg.hardware_device_states + ): + return + + try: + dt_object = datetime.datetime.fromtimestamp(msg.header.stamp.sec) + nano_str = f"{msg.header.stamp.nanosec:09d}" + timestamp = f"{dt_object.strftime('%H:%M:%S')}.{nano_str[:3]}" + except (ValueError, OSError): + timestamp = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}" + + print( + f"{bcolors.OKGREEN}Hardware ID: {bcolors.ENDC}{msg.hardware_id} ({bcolors.WARNING}stamp: {timestamp}{bcolors.ENDC})" + ) + + for device_state in msg.hardware_device_states: + if args.device_id and device_state.device_id != args.device_id: + continue + + print(f" {bcolors.OKCYAN}Device ID: {bcolors.ENDC}{device_state.device_id}") + + state_types = [ + ("Generic Hardware States", device_state.hardware_status), + ("CANopen States", device_state.canopen_states), + ("EtherCAT States", device_state.ethercat_states), + ("VDA5050 States", device_state.vda5050_states), + ] + + any_state_printed = False + for title, states in state_types: + if states: + any_state_printed = True + print(f" {bcolors.OKBLUE}{title}:{bcolors.ENDC}") + for state in states: + print(" -") + yaml_str = message_to_yaml(state, flow_style=False) + indented_str = "\n".join( + [f" {line}" for line in yaml_str.splitlines()] + ) + print(indented_str) + + if not any_state_printed: + print(f" {bcolors.FAIL}Status: No specific states reported{bcolors.ENDC}") + + print("---") + + def main(self, *, args): + with NodeStrategy(args).direct_node as node: + topic_names_and_types = get_topic_names_and_types( + node=node, include_hidden_topics=True + ) + + status_topics = sorted( + [ + name + for name, types in topic_names_and_types + if name.endswith("/hardware_status") + and "control_msgs/msg/HardwareStatus" in types + ] + ) + + if not status_topics: + print( + f"{bcolors.FAIL}No topics of type 'control_msgs/msg/HardwareStatus' found.{bcolors.ENDC}" + ) + return 1 + + print(f"{bcolors.OKBLUE}Subscribing to the following topics:{bcolors.ENDC}") + for topic in status_topics: + print(f"\t{topic}") + print("---") + + _ = [ + node.create_subscription( + HardwareStatus, topic, partial(self._on_message, args=args), 10 + ) + for topic in status_topics + ] + + rclpy.spin(node) + + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index e20a1519e5..4394c1755f 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.29.0", + version="6.0.1", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), @@ -36,14 +36,17 @@ classifiers=[ "Environment :: Console", "Intended Audience :: Developers", - "License :: OSI Approved :: Apache Software License", "Programming Language :: Python", ], description="ROS2 Control command interface.", long_description="""\ ROS2 Control command interface.""", license="Apache License, Version 2.0", - tests_require=["pytest"], + extras_require={ + "test": [ + "pytest", + ], + }, entry_points={ "ros2cli.command": [ "control = ros2controlcli.command.control:ControlCommand", @@ -66,6 +69,7 @@ ros2controlcli.verb.set_hardware_component_state:SetHardwareComponentStateVerb", "switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb", "unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb", + "view_hardware_status = ros2controlcli.verb.view_hardware_status:ViewHardwareStatusVerb", ], }, ) diff --git a/ros_controls.humble.repos b/ros_controls.humble.repos index a9ad96bff4..c14d86b031 100644 --- a/ros_controls.humble.repos +++ b/ros_controls.humble.repos @@ -1,21 +1,26 @@ repositories: - ros-controls/gz_ros2_control: + gz_ros2_control: type: git url: https://github.com/ros-controls/gz_ros2_control.git version: humble - ros-controls/ros2_control_demos: + ros2_control_demos: type: git url: https://github.com/ros-controls/ros2_control_demos.git version: humble - ros-controls/ros2_controllers: + ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git version: humble - ros-controls/control_toolbox: + control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git version: humble - ros-controls/kinematics_interface: + kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git version: humble + # deactivate until https://github.com/ros-controls/topic_based_hardware_interfaces/issues/9 is fixed + # ros-controls/topic_based_hardware_interfaces: + # type: git + # url: https://github.com/ros-controls/topic_based_hardware_interfaces.git + # version: main diff --git a/ros_controls.jazzy.repos b/ros_controls.jazzy.repos index 8a7249c0c2..addbd40f35 100644 --- a/ros_controls.jazzy.repos +++ b/ros_controls.jazzy.repos @@ -1,21 +1,25 @@ repositories: - ros-controls/gz_ros2_control: + gz_ros2_control: type: git url: https://github.com/ros-controls/gz_ros2_control.git version: jazzy - ros-controls/ros2_control_demos: + ros2_control_demos: type: git url: https://github.com/ros-controls/ros2_control_demos.git - version: master - ros-controls/ros2_controllers: + version: jazzy + ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git - version: master - ros-controls/control_toolbox: + version: jazzy + control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git version: jazzy - ros-controls/kinematics_interface: + kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git version: jazzy + topic_based_hardware_interfaces: + type: git + url: https://github.com/ros-controls/topic_based_hardware_interfaces.git + version: main diff --git a/ros_controls.kilted.repos b/ros_controls.kilted.repos new file mode 100644 index 0000000000..e6a4eafee1 --- /dev/null +++ b/ros_controls.kilted.repos @@ -0,0 +1,25 @@ +repositories: + gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: rolling + ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: kilted + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + topic_based_hardware_interfaces: + type: git + url: https://github.com/ros-controls/topic_based_hardware_interfaces.git + version: main diff --git a/ros_controls.rolling.repos b/ros_controls.rolling.repos index e13b0f7bba..63d3e54584 100644 --- a/ros_controls.rolling.repos +++ b/ros_controls.rolling.repos @@ -1,21 +1,25 @@ repositories: - ros-controls/gz_ros2_control: + gz_ros2_control: type: git url: https://github.com/ros-controls/gz_ros2_control.git version: rolling - ros-controls/ros2_control_demos: + ros2_control_demos: type: git url: https://github.com/ros-controls/ros2_control_demos.git version: master - ros-controls/ros2_controllers: + ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git version: master - ros-controls/control_toolbox: + control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master - ros-controls/kinematics_interface: + version: master + kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + topic_based_hardware_interfaces: + type: git + url: https://github.com/ros-controls/topic_based_hardware_interfaces.git + version: main diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 569d69bc09..505d76d7cd 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ + +5.7.0 (2025-10-03) +------------------ + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ +* Fix wrong strict arg of switch_controllers method (`#2410 `_) +* Contributors: Sai Kishor Kothakota + +5.4.0 (2025-07-21) +------------------ +* Fix setuptools deprecations (`#2395 `_) +* Contributors: mosfet80 + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ + +5.1.0 (2025-05-24) +------------------ + +5.0.0 (2025-05-21) +------------------ + 4.29.0 (2025-05-04) ------------------- * Add consistent transitions for controllers in rqt_controller_manager (`#2163 `_) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 3b1ef23c0e..ea9fe9a000 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.29.0 + 6.0.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 50120c32b4..7471e188fc 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -444,7 +444,7 @@ def _switch_controllers(self, activate, deactivate): controller_manager_name=self._cm_name, activate_controllers=activate, deactivate_controllers=deactivate, - strict=SwitchController.Request.STRICT, + strictness=SwitchController.Request.STRICT, activate_asap=False, timeout=0.3, ) diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 7f843f9294..ee6f313183 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.29.0", + version="6.0.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -34,7 +34,11 @@ maintainer_email="bence.magyar.robotics@gmail.com", description="Graphical frontend for interacting with the controller manager.", license="Apache License, Version 2.0", - tests_require=["pytest"], + extras_require={ + "test": [ + "pytest", + ], + }, entry_points={ "console_scripts": [ "rqt_controller_manager = \ diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index eaeb6f38a8..3fbef9320c 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.1 (2025-11-03) +------------------ + +6.0.0 (2025-10-27) +------------------ +* [Transmission] Fix the differential transmission configure checks (`#2682 `_) +* [Transmissions] Add `force` interface (`#2588 `_) +* Contributors: Jordan Palacios, Sai Kishor Kothakota + +5.7.0 (2025-10-03) +------------------ + +5.6.0 (2025-08-26) +------------------ + +5.5.0 (2025-07-31) +------------------ + +5.4.0 (2025-07-21) +------------------ + +5.3.0 (2025-07-02) +------------------ + +5.2.0 (2025-06-07) +------------------ +* [Transmissions] Add `absolute_position` and `torque` interfaces (`#2310 `_) +* Fix pre-commit (`#2277 `_) +* Fix fourbarlinkage (`#1837 `_) +* Contributors: Bartłomiej Krajewski, Jordan Palacios, Sai Kishor Kothakota + +5.1.0 (2025-05-24) +------------------ +* Use target_link_libraries instead of ament_target_dependencies (`#2266 `_) +* Contributors: Sai Kishor Kothakota + +5.0.0 (2025-05-21) +------------------ +* Statically allocate string concatenations using FMT formatting (`#2205 `_) +* Suppress the deprecation warnings of the hardware_interface API (`#2223 `_) +* Contributors: Sai Kishor Kothakota, mini-1235 + 4.29.0 (2025-05-04) ------------------- diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 3cd2e80dd2..4f758cc3f3 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -9,6 +9,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface pluginlib rclcpp + fmt ) find_package(ament_cmake REQUIRED) @@ -26,7 +27,11 @@ target_include_directories(transmission_interface PUBLIC $ $ ) -ament_target_dependencies(transmission_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(transmission_interface PUBLIC + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + fmt::fmt) pluginlib_export_plugin_description_file(transmission_interface ros2_control_plugins.xml) if(BUILD_TESTING) @@ -51,27 +56,30 @@ if(BUILD_TESTING) ament_add_gmock(test_simple_transmission_loader test/simple_transmission_loader_test.cpp ) - target_link_libraries(test_simple_transmission_loader transmission_interface) - ament_target_dependencies(test_simple_transmission_loader ros2_control_test_assets) + target_link_libraries(test_simple_transmission_loader + transmission_interface + ros2_control_test_assets::ros2_control_test_assets) ament_add_gmock(test_four_bar_linkage_transmission_loader test/four_bar_linkage_transmission_loader_test.cpp ) - target_link_libraries(test_four_bar_linkage_transmission_loader transmission_interface) - ament_target_dependencies(test_four_bar_linkage_transmission_loader ros2_control_test_assets) + target_link_libraries(test_four_bar_linkage_transmission_loader + transmission_interface + ros2_control_test_assets::ros2_control_test_assets) ament_add_gmock(test_differential_transmission_loader test/differential_transmission_loader_test.cpp ) - target_link_libraries(test_differential_transmission_loader transmission_interface) - ament_target_dependencies(test_differential_transmission_loader ros2_control_test_assets) + target_link_libraries(test_differential_transmission_loader + transmission_interface + ros2_control_test_assets::ros2_control_test_assets) ament_add_gmock( test_utils test/utils_test.cpp ) target_include_directories(test_utils PUBLIC include hardware_interface) - ament_target_dependencies(test_utils hardware_interface) + target_link_libraries(test_utils hardware_interface::hardware_interface) endif() install( diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index b6b4353dd8..5dc61dc795 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -15,6 +15,8 @@ #ifndef TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_ #define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_ +#include + #include #include #include @@ -106,6 +108,9 @@ namespace transmission_interface * * \ingroup transmission_types */ + +constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position"; + class DifferentialTransmission : public Transmission { public: @@ -162,10 +167,16 @@ class DifferentialTransmission : public Transmission std::vector joint_position_; std::vector joint_velocity_; std::vector joint_effort_; + std::vector joint_torque_; + std::vector joint_force_; + std::vector joint_absolute_position_; std::vector actuator_position_; std::vector actuator_velocity_; std::vector actuator_effort_; + std::vector actuator_torque_; + std::vector actuator_force_; + std::vector actuator_absolute_position_; }; inline DifferentialTransmission::DifferentialTransmission( @@ -208,14 +219,17 @@ void DifferentialTransmission::configure( if (joint_names.size() != 2) { throw Exception( - "There should be exactly two unique joint names but was given " + to_string(joint_names)); + fmt::format( + FMT_COMPILE("There should be exactly two unique joint names but was given '{}'."), + to_string(joint_names))); } const auto actuator_names = get_names(actuator_handles); if (actuator_names.size() != 2) { throw Exception( - "There should be exactly two unique actuator names but was given " + - to_string(actuator_names)); + fmt::format( + FMT_COMPILE("There should be exactly two unique actuator names but was given '{}'."), + to_string(actuator_names))); } joint_position_ = @@ -223,10 +237,53 @@ void DifferentialTransmission::configure( joint_velocity_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY); joint_effort_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_EFFORT); + joint_torque_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_TORQUE); + joint_force_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_FORCE); + joint_absolute_position_ = + get_ordered_handles(joint_handles, joint_names, HW_IF_ABSOLUTE_POSITION); - if (joint_position_.size() != 2 && joint_velocity_.size() != 2 && joint_effort_.size() != 2) + if (!joint_position_.empty() && joint_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint position handles were present. \n{}"), + get_handles_info())); + } + if (!joint_velocity_.empty() && joint_velocity_.size() != 2) { - throw Exception("Not enough valid or required joint handles were presented."); + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint velocity handles were present. \n{}"), + get_handles_info())); + } + if (!joint_effort_.empty() && joint_effort_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint effort handles were present. \n{}"), + get_handles_info())); + } + if (!joint_torque_.empty() && joint_torque_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint torque handles were present. \n{}"), + get_handles_info())); + } + if (!joint_force_.empty() && joint_force_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint force handles were present. \n{}"), + get_handles_info())); + } + if (!joint_absolute_position_.empty() && joint_absolute_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE( + "Not enough valid or required joint absolute position handles were present. \n{}"), + get_handles_info())); } actuator_position_ = @@ -235,21 +292,93 @@ void DifferentialTransmission::configure( get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY); actuator_effort_ = get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT); - - if ( - actuator_position_.size() != 2 && actuator_velocity_.size() != 2 && - actuator_effort_.size() != 2) + actuator_torque_ = + get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_TORQUE); + actuator_force_ = + get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_FORCE); + actuator_absolute_position_ = + get_ordered_handles(actuator_handles, actuator_names, HW_IF_ABSOLUTE_POSITION); + + if (!actuator_position_.empty() && actuator_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator position handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_velocity_.empty() && actuator_velocity_.size() != 2) { throw Exception( - "Not enough valid or required actuator handles were presented. \n" + get_handles_info()); + fmt::format( + FMT_COMPILE("Not enough valid or required actuator velocity handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_effort_.empty() && actuator_effort_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator effort handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_torque_.empty() && actuator_torque_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator torque handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_force_.empty() && actuator_force_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator force handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_absolute_position_.empty() && actuator_absolute_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE( + "Not enough valid or required actuator absolute position handles were " + "present. \n{}"), + get_handles_info())); } - if ( - joint_position_.size() != actuator_position_.size() && - joint_velocity_.size() != actuator_velocity_.size() && - joint_effort_.size() != actuator_effort_.size()) + if (joint_position_.size() != actuator_position_.size()) { - throw Exception("Pair-wise mismatch on interfaces. \n" + get_handles_info()); + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on position interfaces. \n{}"), get_handles_info())); + } + if (joint_velocity_.size() != actuator_velocity_.size()) + { + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on velocity interfaces. \n{}"), get_handles_info())); + } + if (joint_effort_.size() != actuator_effort_.size()) + { + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on effort interfaces. \n{}"), get_handles_info())); + } + if (joint_torque_.size() != actuator_torque_.size()) + { + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on torque interfaces. \n{}"), get_handles_info())); + } + if (joint_force_.size() != actuator_force_.size()) + { + throw Exception( + fmt::format(FMT_COMPILE("Pair-wise mismatch on force interfaces. \n{}"), get_handles_info())); + } + if (joint_absolute_position_.size() != actuator_absolute_position_.size()) + { + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on absolute position interfaces. \n{}"), + get_handles_info())); } } @@ -295,6 +424,44 @@ inline void DifferentialTransmission::actuator_to_joint() joint_eff[1].set_value( jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); } + + auto & act_tor = actuator_torque_; + auto & joint_tor = joint_torque_; + if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints()) + { + assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]); + + joint_tor[0].set_value( + jr[0] * (act_tor[0].get_value() * ar[0] + act_tor[1].get_value() * ar[1])); + joint_tor[1].set_value( + jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[1].get_value() * ar[1])); + } + + auto & act_for = actuator_force_; + auto & joint_for = joint_force_; + if (act_for.size() == num_actuators() && joint_for.size() == num_joints()) + { + assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]); + + joint_for[0].set_value( + jr[0] * (act_for[0].get_value() * ar[0] + act_for[1].get_value() * ar[1])); + joint_for[1].set_value( + jr[1] * (act_for[0].get_value() * ar[0] - act_for[1].get_value() * ar[1])); + } + + auto & act_abs_pos = actuator_absolute_position_; + auto & joint_abs_pos = joint_absolute_position_; + if (act_abs_pos.size() == num_actuators() && joint_abs_pos.size() == num_joints()) + { + assert(act_abs_pos[0] && act_abs_pos[1] && joint_abs_pos[0] && joint_abs_pos[1]); + + joint_abs_pos[0].set_value( + (act_abs_pos[0].get_value() / ar[0] + act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + + joint_offset_[0]); + joint_abs_pos[1].set_value( + (act_abs_pos[0].get_value() / ar[0] - act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) + + joint_offset_[1]); + } } inline void DifferentialTransmission::joint_to_actuator() @@ -339,17 +506,50 @@ inline void DifferentialTransmission::joint_to_actuator() act_eff[1].set_value( (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1])); } + + auto & act_tor = actuator_torque_; + auto & joint_tor = joint_torque_; + if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints()) + { + assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]); + + act_tor[0].set_value( + (joint_tor[0].get_value() / jr[0] + joint_tor[1].get_value() / jr[1]) / (2.0 * ar[0])); + act_tor[1].set_value( + (joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1])); + } + + auto & act_for = actuator_force_; + auto & joint_for = joint_force_; + if (act_for.size() == num_actuators() && joint_for.size() == num_joints()) + { + assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]); + + act_for[0].set_value( + (joint_for[0].get_value() / jr[0] + joint_for[1].get_value() / jr[1]) / (2.0 * ar[0])); + act_for[1].set_value( + (joint_for[0].get_value() / jr[0] - joint_for[1].get_value() / jr[1]) / (2.0 * ar[1])); + } } std::string DifferentialTransmission::get_handles_info() const { - return std::string("Got the following handles:\n") + - "Joint position: " + to_string(get_names(joint_position_)) + - ", Actuator position: " + to_string(get_names(actuator_position_)) + "\n" + - "Joint velocity: " + to_string(get_names(joint_velocity_)) + - ", Actuator velocity: " + to_string(get_names(actuator_velocity_)) + "\n" + - "Joint effort: " + to_string(get_names(joint_effort_)) + - ", Actuator effort: " + to_string(get_names(actuator_effort_)); + return fmt::format( + FMT_COMPILE( + "Got the following handles:\n" + "Joint position: {}, Actuator position: {}\n" + "Joint velocity: {}, Actuator velocity: {}\n" + "Joint effort: {}, Actuator effort: {}\n" + "Joint torque: {}, Actuator torque: {}\n" + "Joint force: {}, Actuator force: {}\n" + "Joint absolute position: {}, Actuator absolute position: {}"), + to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)), + to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)), + to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)), + to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)), + to_string(get_names(joint_force_)), to_string(get_names(actuator_force_)), + to_string(get_names(joint_absolute_position_)), + to_string(get_names(actuator_absolute_position_))); } } // namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 107133128c..243d3dfc0e 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -17,6 +17,8 @@ #ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_ #define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_ +#include + #include #include #include @@ -45,8 +47,8 @@ namespace transmission_interface * * * \f{eqnarray*}{ - * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ - * \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) + * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2} \\ + * \tau_{j_2} & = & n_{j_2} n_{a_2} \tau_{a_2}2} * \f} * * @@ -67,8 +69,8 @@ namespace transmission_interface * * * \f{eqnarray*}{ - * \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ - * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } + * \tau_{a_1} & = & \frac{\tau_{j_1} - \tau_{j_2}/n_{j_2}} {n_{j_1} n_{a_1}} \\ + * \tau_{a_2} & = & \frac{\tau_{j_2}} {n_{j_2} n_{a_2}} * \f} * * @@ -205,14 +207,17 @@ void FourBarLinkageTransmission::configure( if (joint_names.size() != 2) { throw Exception( - "There should be exactly two unique joint names but was given " + to_string(joint_names)); + fmt::format( + FMT_COMPILE("There should be exactly two unique joint names but was given '{}'."), + to_string(joint_names))); } const auto actuator_names = get_names(actuator_handles); if (actuator_names.size() != 2) { throw Exception( - "There should be exactly two unique actuator names but was given " + - to_string(actuator_names)); + fmt::format( + FMT_COMPILE("There should be exactly two unique actuator names but was given '{}'."), + to_string(actuator_names))); } joint_position_ = @@ -238,7 +243,9 @@ void FourBarLinkageTransmission::configure( actuator_effort_.size() != 2) { throw Exception( - "Not enough valid or required actuator handles were presented. \n" + get_handles_info()); + fmt::format( + FMT_COMPILE("Not enough valid or required actuator handles were presented. \n{}"), + get_handles_info())); } if ( @@ -246,7 +253,8 @@ void FourBarLinkageTransmission::configure( joint_velocity_.size() != actuator_velocity_.size() && joint_effort_.size() != actuator_effort_.size()) { - throw Exception("Pair-wise mismatch on interfaces. \n" + get_handles_info()); + throw Exception( + fmt::format(FMT_COMPILE("Pair-wise mismatch on interfaces. \n{}"), get_handles_info())); } } @@ -287,9 +295,8 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); - joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0])); + joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1]); + joint_eff[1].set_value(jr[1] * act_eff[1].get_value() * ar[1]); } } @@ -329,20 +336,23 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); - act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); + act_eff[0].set_value( + (joint_eff[0].get_value() - joint_eff[1].get_value() / jr[1]) / (jr[0] * ar[0])); + act_eff[1].set_value(joint_eff[1].get_value() / (ar[1] * jr[1])); } } std::string FourBarLinkageTransmission::get_handles_info() const { - return std::string("Got the following handles:\n") + - "Joint position: " + to_string(get_names(joint_position_)) + - ", Actuator position: " + to_string(get_names(actuator_position_)) + "\n" + - "Joint velocity: " + to_string(get_names(joint_velocity_)) + - ", Actuator velocity: " + to_string(get_names(actuator_velocity_)) + "\n" + - "Joint effort: " + to_string(get_names(joint_effort_)) + - ", Actuator effort: " + to_string(get_names(actuator_effort_)); + return fmt::format( + FMT_COMPILE( + "Got the following handles:\n" + "Joint position: {}, Actuator position: {}\n" + "Joint velocity: {}, Actuator velocity: {}\n" + "Joint effort: {}, Actuator effort: {}"), + to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)), + to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)), + to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_))); } } // namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 4c40189648..a97a6c6757 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -15,22 +15,70 @@ #ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_ #define TRANSMISSION_INTERFACE__HANDLE_HPP_ -#include "hardware_interface/handle.hpp" +#include +#include "hardware_interface/macros.hpp" namespace transmission_interface { +class Handle +{ +public: + Handle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + { + } + + Handle(const Handle & other) = default; + + Handle(Handle && other) = default; + + Handle & operator=(const Handle & other) = default; + + Handle & operator=(Handle && other) = default; + + virtual ~Handle() = default; + + /// Returns true if handle references a value. + inline operator bool() const { return value_ptr_ != nullptr; } + + const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } + + const std::string & get_interface_name() const { return interface_name_; } + + const std::string & get_prefix_name() const { return prefix_name_; } + + double get_value() const + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } + + void set_value(double value) + { + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + } + +protected: + std::string prefix_name_; + std::string interface_name_; + double * value_ptr_ = nullptr; +}; + /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::Handle +class ActuatorHandle : public transmission_interface::Handle { public: - using hardware_interface::Handle::Handle; + using transmission_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::Handle +class JointHandle : public transmission_interface::Handle { public: - using hardware_interface::Handle::Handle; + using transmission_interface::Handle::Handle; }; } // namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab75..1d53557b91 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -80,6 +80,9 @@ namespace transmission_interface * * \ingroup transmission_types */ + +constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position"; + class SimpleTransmission : public Transmission { public: @@ -131,10 +134,16 @@ class SimpleTransmission : public Transmission JointHandle joint_position_ = {"", "", nullptr}; JointHandle joint_velocity_ = {"", "", nullptr}; JointHandle joint_effort_ = {"", "", nullptr}; + JointHandle joint_torque_ = {"", "", nullptr}; + JointHandle joint_force_ = {"", "", nullptr}; + JointHandle joint_absolute_position_ = {"", "", nullptr}; ActuatorHandle actuator_position_ = {"", "", nullptr}; ActuatorHandle actuator_velocity_ = {"", "", nullptr}; ActuatorHandle actuator_effort_ = {"", "", nullptr}; + ActuatorHandle actuator_torque_ = {"", "", nullptr}; + ActuatorHandle actuator_force_ = {"", "", nullptr}; + ActuatorHandle actuator_absolute_position_ = {"", "", nullptr}; }; inline SimpleTransmission::SimpleTransmission( @@ -198,8 +207,13 @@ inline void SimpleTransmission::configure( joint_position_ = get_by_interface(joint_handles, hardware_interface::HW_IF_POSITION); joint_velocity_ = get_by_interface(joint_handles, hardware_interface::HW_IF_VELOCITY); joint_effort_ = get_by_interface(joint_handles, hardware_interface::HW_IF_EFFORT); + joint_torque_ = get_by_interface(joint_handles, hardware_interface::HW_IF_TORQUE); + joint_force_ = get_by_interface(joint_handles, hardware_interface::HW_IF_FORCE); + joint_absolute_position_ = get_by_interface(joint_handles, HW_IF_ABSOLUTE_POSITION); - if (!joint_position_ && !joint_velocity_ && !joint_effort_) + if ( + !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ && !joint_force_ && + !joint_absolute_position_) { throw Exception("None of the provided joint handles are valid or from the required interfaces"); } @@ -207,10 +221,16 @@ inline void SimpleTransmission::configure( actuator_position_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_POSITION); actuator_velocity_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_VELOCITY); actuator_effort_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_EFFORT); + actuator_torque_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_TORQUE); + actuator_force_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_FORCE); + actuator_absolute_position_ = get_by_interface(actuator_handles, HW_IF_ABSOLUTE_POSITION); - if (!actuator_position_ && !actuator_velocity_ && !actuator_effort_) + if ( + !actuator_position_ && !actuator_velocity_ && !actuator_effort_ && !actuator_torque_ && + !actuator_force_ && !actuator_absolute_position_) { - throw Exception("None of the provided joint handles are valid or from the required interfaces"); + throw Exception( + "None of the provided actuator handles are valid or from the required interfaces"); } } @@ -230,6 +250,22 @@ inline void SimpleTransmission::actuator_to_joint() { joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); } + + if (joint_torque_ && actuator_torque_) + { + joint_torque_.set_value(actuator_torque_.get_value() * reduction_); + } + + if (joint_force_ && actuator_force_) + { + joint_force_.set_value(actuator_force_.get_value() * reduction_); + } + + if (joint_absolute_position_ && actuator_absolute_position_) + { + joint_absolute_position_.set_value( + actuator_absolute_position_.get_value() / reduction_ + jnt_offset_); + } } inline void SimpleTransmission::joint_to_actuator() @@ -248,6 +284,16 @@ inline void SimpleTransmission::joint_to_actuator() { actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); } + + if (joint_torque_ && actuator_torque_) + { + actuator_torque_.set_value(joint_torque_.get_value() / reduction_); + } + + if (joint_force_ && actuator_force_) + { + actuator_force_.set_value(joint_force_.get_value() / reduction_); + } } } // namespace transmission_interface diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 4e82791086..3dd2774804 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.29.0 + 6.0.1 data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl @@ -16,6 +16,7 @@ hardware_interface pluginlib + fmt ament_cmake_gmock ros2_control_test_assets diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index d70e85f876..96a6b8e002 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -21,12 +21,15 @@ #include "transmission_interface/differential_transmission.hpp" using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_FORCE; using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_TORQUE; using hardware_interface::HW_IF_VELOCITY; using testing::DoubleNear; using transmission_interface::ActuatorHandle; using transmission_interface::DifferentialTransmission; using transmission_interface::Exception; +using transmission_interface::HW_IF_ABSOLUTE_POSITION; using transmission_interface::JointHandle; // Floating-point value comparison threshold const double EPS = 1e-5; @@ -125,6 +128,9 @@ TEST(ConfigureTest, FailsWithBadHandles) testConfigureWithBadHandles(HW_IF_POSITION); testConfigureWithBadHandles(HW_IF_VELOCITY); testConfigureWithBadHandles(HW_IF_EFFORT); + testConfigureWithBadHandles(HW_IF_TORQUE); + testConfigureWithBadHandles(HW_IF_FORCE); + testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION); } class TransmissionSetup : public ::testing::Test @@ -220,6 +226,8 @@ TEST_F(BlackBoxTest, IdentityMap) testIdentityMap(transmission, input_value, HW_IF_POSITION); testIdentityMap(transmission, input_value, HW_IF_VELOCITY); testIdentityMap(transmission, input_value, HW_IF_EFFORT); + testIdentityMap(transmission, input_value, HW_IF_TORQUE); + testIdentityMap(transmission, input_value, HW_IF_FORCE); } } } @@ -236,7 +244,7 @@ TEST_F(WhiteBoxTest, DontMoveJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); - // Actuator input (used for effort, velocity and position) + // Actuator input (used for effort, velocity, position, torque and absolute position) *a_vec[0] = 0.0; *a_vec[1] = 0.0; @@ -275,6 +283,42 @@ TEST_F(WhiteBoxTest, DontMoveJoints) EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); } + + // Torque interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + } + + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + } + + // Absolute position interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + } } TEST_F(WhiteBoxTest, MoveFirstJointOnly) @@ -284,7 +328,7 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); - // Actuator input (used for effort, velocity and position) + // Actuator input (used for effort, velocity, position, torque and absolute position) *a_vec[0] = 10.0; *a_vec[1] = 10.0; @@ -323,6 +367,42 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); } + + // Torque interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + } + + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + } + + // Absolute position interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + } } TEST_F(WhiteBoxTest, MoveSecondJointOnly) @@ -332,7 +412,7 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); - // Actuator input (used for effort, velocity and position) + // Actuator input (used for effort, velocity, position, torque and absolute position) *a_vec[0] = 10.0; *a_vec[1] = -10.0; @@ -371,6 +451,42 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); } + + // Torque interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); + } + + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); + } + + // Absolute position interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + } } TEST_F(WhiteBoxTest, MoveBothJoints) @@ -424,4 +540,40 @@ TEST_F(WhiteBoxTest, MoveBothJoints) EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS)); } + + // Torque interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + } + + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + } + + // Absolute position interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS)); + } } diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index 64a91a7780..a70b61010b 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -297,8 +297,8 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(200.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS)); } // Velocity interface @@ -349,7 +349,7 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS)); } @@ -402,8 +402,8 @@ TEST_F(WhiteBoxTest, MoveBothJoints) auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-160.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-160.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(-400.0, DoubleNear(j_val[1], EPS)); } // Velocity interface diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 4f068d9d11..cd348e6497 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -19,10 +19,13 @@ #include "transmission_interface/simple_transmission.hpp" using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_FORCE; using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_TORQUE; using hardware_interface::HW_IF_VELOCITY; using transmission_interface::ActuatorHandle; using transmission_interface::Exception; +using transmission_interface::HW_IF_ABSOLUTE_POSITION; using transmission_interface::JointHandle; using transmission_interface::SimpleTransmission; @@ -118,18 +121,15 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam