diff --git a/.clang-format b/.clang-format index ab8d077577..9e92cf780a 100644 --- a/.clang-format +++ b/.clang-format @@ -10,5 +10,5 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 6bbf2e957a..6a829c46f9 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! -- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/humble/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://control.ros.org/humble/doc/getting_started/getting_started.html#building-from-source). - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork) @@ -53,8 +53,8 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! Furthermore, you find helpful resources here: -* [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) -* [ROS2 Tutorials](https://docs.ros.org/en/foxy/Tutorials.html) +* [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) +* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) **Good luck with your first issue!** diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..f5e9921f23 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,20 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "humble" diff --git a/.github/mergify.yml b/.github/mergify.yml index 6bfd4470ac..917c57f427 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,26 +1,33 @@ pull_request_rules: - - name: Backport to galactic at reviewers discretion + + - name: Ask to resolve conflict conditions: - - base=master - - "label=backport-galactic" + - conflict + - author!=mergify actions: - backport: - branches: - - galactic + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? - - name: Backport to foxy at reviewers discretion + - name: Ask to resolve conflict for backports conditions: - - base=master - - "label=backport-foxy" + - conflict + - author=mergify[bot] actions: - backport: - branches: - - foxy + comment: + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - - name: Ask to resolve conflict + - name: development targets master branch conditions: - - conflict - - author!=mergify + - base!=master + - author!=bmagyar + - author!=destogl + - author!=christophfroehlich + - author!=saikishor + - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @{{author}}? + message: | + @{{author}}, all pull requests must be targeted towards the `master` development branch. + Once merged into `master`, it is possible to backport to `{{base}}`, but it must be in `master` + to have these changes reflected into new distributions. diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml deleted file mode 100644 index 50c707e12b..0000000000 --- a/.github/reviewer-lottery.yml +++ /dev/null @@ -1,36 +0,0 @@ -groups: - # Default reviewers for all pull requests. - # Usually, at least on of the maintainers should approve PR before merging. - # The best is if two maintainers do that. - - name: maintainers # name of the group - reviewers: 2 # how many reviewers do you want to assign? - internal_reviewers: 1 # how many reviewers do you want to assign when the PR author belongs to this group? - usernames: # github usernames of the reviewers - - bmagyar - - destogl - - # Reviewers group to get broader feedback. - - name: reviewers - reviewers: 5 - usernames: - - rosterloh - - progtologist - - arne48 - - DasRoteSkelett - - Serafadam - - harderthan - - jaron-l - - malapatiravi - - homalozoa - - erickisos - - anfemosa - - jackcenter - - VX792 - - mhubii - - livanov93 - - aprotyas - - peterdavidfagan - - duringhof - - bijoua29 - - lm2292 - - mcbed diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml deleted file mode 100644 index d9b4ec4494..0000000000 --- a/.github/workflows/ci-coverage-build.yml +++ /dev/null @@ -1,48 +0,0 @@ -name: Coverage Build -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: rolling - steps: - - uses: ros-tooling/setup-ros@0.3.4 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed in the meta package - package-name: - diff_drive_controller - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.0 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.0 - with: - name: colcon-logs-coverage-rolling - path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml deleted file mode 100644 index dd78cca0ed..0000000000 --- a/.github/workflows/ci-format.yml +++ /dev/null @@ -1,22 +0,0 @@ -# This is a format job. Pre-commit has a first-party GitHub action, so we use -# that: https://github.com/pre-commit/action - -name: Format - -on: - pull_request: - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 - with: - python-version: 3.9 - - name: Install system hooks - run: sudo apt install -qq clang-format-12 cppcheck - - uses: pre-commit/action@v2.0.3 - with: - extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml deleted file mode 100644 index d8f77a323a..0000000000 --- a/.github/workflows/ci-ros-lint.yml +++ /dev/null @@ -1,62 +0,0 @@ -name: ROS Lint -on: - pull_request: - -jobs: - ament_lint: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 - strategy: - fail-fast: false - matrix: - linter: [cppcheck, copyright, lint_cmake] - steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: ${{ matrix.linter }} - package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - tricycle_controller - velocity_controllers - - - ament_lint_100: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 - strategy: - fail-fast: false - matrix: - linter: [cpplint] - steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - tricycle_controller - velocity_controllers diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml deleted file mode 100644 index 7ce17effd0..0000000000 --- a/.github/workflows/foxy-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Foxy - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: foxy - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build-main.yml b/.github/workflows/foxy-binary-build-main.yml deleted file mode 100644 index b306d7e44d..0000000000 --- a/.github/workflows/foxy-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: main - upstream_workspace: ros2_controllers-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-binary-build-testing.yml b/.github/workflows/foxy-binary-build-testing.yml deleted file mode 100644 index 260751abea..0000000000 --- a/.github/workflows/foxy-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-main.yml b/.github/workflows/foxy-semi-binary-build-main.yml deleted file mode 100644 index 75c3295124..0000000000 --- a/.github/workflows/foxy-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: main - upstream_workspace: ros2_controllers.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-testing.yml b/.github/workflows/foxy-semi-binary-build-testing.yml deleted file mode 100644 index f6d663cc2c..0000000000 --- a/.github/workflows/foxy-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: testing - upstream_workspace: ros2_controllers.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml deleted file mode 100644 index 06a48ef9c7..0000000000 --- a/.github/workflows/galactic-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Galactic - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: galactic - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-binary-build-main.yml b/.github/workflows/galactic-binary-build-main.yml deleted file mode 100644 index 14fe56407c..0000000000 --- a/.github/workflows/galactic-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Galactic Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: galactic - ros_repo: main - upstream_workspace: ros2_controllers-not-released.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-binary-build-testing.yml b/.github/workflows/galactic-binary-build-testing.yml deleted file mode 100644 index c4b22a3a7a..0000000000 --- a/.github/workflows/galactic-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Galactic Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: galactic - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml deleted file mode 100644 index 518fcc186a..0000000000 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Galactic RHEL Binary Build -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - galactic_rhel_binary: - name: Galactic RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: galactic - container: ghcr.io/ros-controls/ros:galactic-rhel - steps: - - uses: actions/checkout@v3 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test diff --git a/.github/workflows/galactic-semi-binary-build-main.yml b/.github/workflows/galactic-semi-binary-build-main.yml deleted file mode 100644 index 7e498679cb..0000000000 --- a/.github/workflows/galactic-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Galactic Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: galactic - ros_repo: main - upstream_workspace: ros2_controllers.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-semi-binary-build-testing.yml b/.github/workflows/galactic-semi-binary-build-testing.yml deleted file mode 100644 index 82a74a358b..0000000000 --- a/.github/workflows/galactic-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Galactic Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: galactic - ros_repo: testing - upstream_workspace: ros2_controllers.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 1be00cfc76..4023b42a7b 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -1,20 +1,28 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: - branches: - - master pull_request: branches: - - master + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: humble - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: humble diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml deleted file mode 100644 index 01708cf864..0000000000 --- a/.github/workflows/humble-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Humble Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: humble - ros_repo: main - upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml deleted file mode 100644 index 73ed0a4859..0000000000 --- a/.github/workflows/humble-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Humble Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: humble - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml new file mode 100644 index 0000000000..6e6e062f62 --- /dev/null +++ b/.github/workflows/humble-binary-build.yml @@ -0,0 +1,55 @@ +name: Humble Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' + - '**.xml' + push: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' + - '**.xml' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 2 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: humble + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml new file mode 100644 index 0000000000..448ff4084b --- /dev/null +++ b/.github/workflows/humble-check-docs.yml @@ -0,0 +1,22 @@ +name: Humble Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +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@humble + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml new file mode 100644 index 0000000000..55d4aa0cfd --- /dev/null +++ b/.github/workflows/humble-coverage-build.yml @@ -0,0 +1,45 @@ +name: Coverage Build - Humble +on: + workflow_dispatch: + push: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + coverage_humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml new file mode 100644 index 0000000000..e13f30a8ca --- /dev/null +++ b/.github/workflows/humble-debian-build.yml @@ -0,0 +1,35 @@ +name: Debian Humble Build +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + humble_debian: + name: Humble debian build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml new file mode 100644 index 0000000000..38a76ee025 --- /dev/null +++ b/.github/workflows/humble-pre-commit.yml @@ -0,0 +1,17 @@ +name: Pre-Commit - Humble + +on: + workflow_dispatch: + pull_request: + branches: + - humble + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml deleted file mode 100644 index 8f4a65a9b5..0000000000 --- a/.github/workflows/humble-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Humble RHEL Binary Build -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - humble_rhel_binary: - name: Humble RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:humble-rhel - steps: - - uses: actions/checkout@v3 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml new file mode 100644 index 0000000000..c868c60db3 --- /dev/null +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -0,0 +1,35 @@ +name: RHEL Humble Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 2 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + humble_rhel_binary: + name: Humble RHEL binary build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml deleted file mode 100644 index 10b1186b79..0000000000 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Humble Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: humble - ros_repo: main - upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml deleted file mode 100644 index ec73cc6b98..0000000000 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Humble Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: humble - ros_repo: testing - upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml new file mode 100644 index 0000000000..589c816aba --- /dev/null +++ b/.github/workflows/humble-semi-binary-build.yml @@ -0,0 +1,61 @@ +name: Humble Semi-Binary Build +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' + push: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 2 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + semi_binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..a9ba472ff1 --- /dev/null +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -0,0 +1,50 @@ +name: Humble Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.humble.repos' + - '**.xml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.humble.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.humble.repos + not_test_downstream: true diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 18c33b6d52..22c9b50c2c 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,19 +1,29 @@ name: Humble Source Build on: workflow_dispatch: - branches: - - master push: branches: - - master + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 4 * * *' jobs: source: - uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: humble - ref: master + ref: humble ros2_repo_branch: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml new file mode 100644 index 0000000000..82fadb55a1 --- /dev/null +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -0,0 +1,27 @@ +name: Jazzy - ABI Compatibility Check +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + abi_check: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml new file mode 100644 index 0000000000..fb68d930e4 --- /dev/null +++ b/.github/workflows/jazzy-binary-build.yml @@ -0,0 +1,56 @@ +name: Jazzy Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + - '**.xml' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + - '**.xml' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: jazzy + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers-not-released.jazzy.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml new file mode 100644 index 0000000000..cfc67a3eac --- /dev/null +++ b/.github/workflows/jazzy-check-docs.yml @@ -0,0 +1,22 @@ +name: Jazzy Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +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@jazzy + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml new file mode 100644 index 0000000000..2f587c7003 --- /dev/null +++ b/.github/workflows/jazzy-coverage-build.yml @@ -0,0 +1,46 @@ +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' + # - '**.yaml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'codecov.yml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'ros2_controllers.jazzy.repos' + # pull_request: + # branches: + # - master + # paths: + # - '**.hpp' + # - '**.h' + # - '**.cpp' + # - '.github/workflows/jazzy-coverage-build.yml' + # - '**.yaml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'codecov.yml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'ros2_controllers.jazzy.repos' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + coverage_jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml new file mode 100644 index 0000000000..ef1b42da8d --- /dev/null +++ b/.github/workflows/jazzy-debian-build.yml @@ -0,0 +1,35 @@ +name: Debian Jazzy Source Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + jazzy_debian: + name: Rolling debian build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + with: + ros_distro: jazzy + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml new file mode 100644 index 0000000000..aab5ba52ac --- /dev/null +++ b/.github/workflows/jazzy-pre-commit.yml @@ -0,0 +1,17 @@ +name: Pre-Commit - Jazzy + +on: + workflow_dispatch: + pull_request: + branches: + - master + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml new file mode 100644 index 0000000000..ed09d0efb9 --- /dev/null +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -0,0 +1,34 @@ +name: RHEL Jazzy Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + jazzy_rhel: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + with: + ros_distro: jazzy + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml new file mode 100644 index 0000000000..5a39ea5393 --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -0,0 +1,63 @@ +name: Jazzy Semi-Binary Build +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + - '**.xml' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + - '**.xml' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + semi_binary: + 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_controllers.jazzy.repos + ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..a2db0ef7b6 --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -0,0 +1,50 @@ +name: Jazzy Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.jazzy.repos' + - '**.xml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.jazzy.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.jazzy.repos + not_test_downstream: true diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml new file mode 100644 index 0000000000..76c9bd2892 --- /dev/null +++ b/.github/workflows/jazzy-source-build.yml @@ -0,0 +1,29 @@ +name: Jazzy Source Build +on: + workflow_dispatch: + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +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: jazzy + container: ubuntu:24.04 diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index b9460bda47..809471897d 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -28,7 +28,7 @@ jobs: pre_release: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ github.event.inputs.branch }} - name: industrial_ci diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml deleted file mode 100644 index 490b680e7c..0000000000 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ /dev/null @@ -1,96 +0,0 @@ -name: Reusable industrial_ci Workflow with Cache -# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache -# author: Denis Štogl - -on: - workflow_call: - inputs: - ref_for_scheduled_build: - description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' - default: '' - required: false - type: string - - upstream_workspace: - description: 'UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file.' - required: true - type: string - ros_distro: - description: 'ROS_DISTRO variable for industrial_ci' - required: true - type: string - ros_repo: - description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' - default: 'main' - required: false - type: string - os_code_name: - description: 'OS_CODE_NAME variable for industrial_ci' - default: '' - required: false - type: string - before_install_upstream_dependencies: - description: 'BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci' - default: '' - required: false - type: string - - ccache_dir: - description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.ccache' - required: false - type: string - basedir: - description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.work' - required: false - type: string - - -jobs: - reusable_industrial_ci_with_cache: - name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} - runs-on: ubuntu-latest - env: - CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} - BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} - steps: - - name: Checkout ${{ inputs.ref }} when build is not scheduled - if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v3 - - name: Checkout ${{ inputs.ref }} on scheduled build - if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v3 - with: - ref: ${{ inputs.ref_for_scheduled_build }} - - name: cache target_ws - if: ${{ ! matrix.env.CCOV }} - uses: pat-s/always-upload-cache@v2.1.5 - with: - path: ${{ env.BASEDIR }}/target_ws - key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} - restore-keys: | - target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - - name: cache ccache - uses: pat-s/always-upload-cache@v2.1.5 - with: - path: ${{ env.CCACHE_DIR }} - key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} - restore-keys: | - ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} - ccache-${{ env.CACHE_PREFIX }} - - uses: 'ros-industrial/industrial_ci@master' - env: - UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} - ROS_DISTRO: ${{ inputs.ros_distro }} - ROS_REPO: ${{ inputs.ros_repo }} - OS_CODE_NAME: ${{ inputs.os_code_name }} - BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} - - name: prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV }} - run: | - du -sh ${{ env.BASEDIR }}/target_ws - sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete - sudo rm -rf ${{ env.BASEDIR }}/target_ws/src - du -sh ${{ env.BASEDIR }}/target_ws diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml deleted file mode 100644 index 2ad549b0f8..0000000000 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ /dev/null @@ -1,49 +0,0 @@ -name: Reusable industrial_ci Workflow with Cache -# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache -# author: Denis Štogl - -on: - workflow_call: - inputs: - ros_distro: - description: 'ROS2 distribution name' - required: true - type: string - ref: - description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' - required: true - type: string - ros2_repo_branch: - description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' - default: 'master' - required: false - type: string - -jobs: - reusable_ros_tooling_source_build: - name: ${{ inputs.ros_distro }} ubuntu-22.04 - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - steps: - - uses: ros-tooling/setup-ros@0.3.4 - with: - required-ros-distributions: ${{ inputs.ros_distro }} - - uses: actions/checkout@v3 - with: - ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.2.6 - with: - target-ros2-distro: ${{ inputs.ros_distro }} - # build all packages listed in the meta package - package-name: - diff_drive_controller - - vcs-repo-file-url: | - https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v1 - with: - name: colcon-logs-ubuntu-22.04 - path: ros_ws/log diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index f7ae929e5b..0584f4a7f3 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -1,13 +1,10 @@ name: Reviewer lottery +# pull_request_target takes the same events as pull_request, +# but it runs on the base branch instead of the head branch. on: pull_request_target: types: [opened, ready_for_review, reopened] jobs: - test: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v1 - - uses: uesteibar/reviewer-lottery@v2 - with: - repo-token: ${{ secrets.GITHUB_TOKEN }} + assign_reviewers: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-reviewer-lottery.yml@master diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 4589e92e3b..e3cd45c970 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,20 +1,27 @@ name: Rolling - ABI Compatibility Check on: workflow_dispatch: - branches: - - master pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: rolling - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: rolling diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml deleted file mode 100644 index b51bbabe29..0000000000 --- a/.github/workflows/rolling-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Rolling Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: rolling - ros_repo: main - upstream_workspace: ros2_controllers-not-released.rolling.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml deleted file mode 100644 index a1db89b8d9..0000000000 --- a/.github/workflows/rolling-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Rolling Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: rolling - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.rolling.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml new file mode 100644 index 0000000000..1fd126f43c --- /dev/null +++ b/.github/workflows/rolling-binary-build.yml @@ -0,0 +1,57 @@ +name: Rolling Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' + - '**.xml' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' + - '**.xml' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: rolling + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers-not-released.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml new file mode 100644 index 0000000000..5cf79065f6 --- /dev/null +++ b/.github/workflows/rolling-check-docs.yml @@ -0,0 +1,22 @@ +name: Rolling Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +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@master + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..54f85ecf4b --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,50 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers.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 new file mode 100644 index 0000000000..a3f406fe87 --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,50 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml new file mode 100644 index 0000000000..cd40b1954d --- /dev/null +++ b/.github/workflows/rolling-coverage-build.yml @@ -0,0 +1,45 @@ +name: Coverage Build - Rolling +on: + workflow_dispatch: + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + coverage_rolling: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: rolling diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml new file mode 100644 index 0000000000..41a0188df8 --- /dev/null +++ b/.github/workflows/rolling-debian-build.yml @@ -0,0 +1,35 @@ +name: Debian Rolling Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + rolling_debian: + name: Rolling debian build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml new file mode 100644 index 0000000000..792278d6d2 --- /dev/null +++ b/.github/workflows/rolling-pre-commit.yml @@ -0,0 +1,17 @@ +name: Pre-Commit - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - master + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: rolling diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml deleted file mode 100644 index c418319cd7..0000000000 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Rolling RHEL Binary Build -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - rolling_rhel_binary: - name: Rolling RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:rolling-rhel - steps: - - uses: actions/checkout@v3 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml new file mode 100644 index 0000000000..c1abdb3c5c --- /dev/null +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -0,0 +1,34 @@ +name: RHEL Rolling Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + rolling_rhel: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml deleted file mode 100644 index eca9483438..0000000000 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Rolling Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: rolling - ros_repo: main - upstream_workspace: ros2_controllers.rolling.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml deleted file mode 100644 index 62214adae9..0000000000 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Rolling Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: rolling - ros_repo: testing - upstream_workspace: ros2_controllers.rolling.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml new file mode 100644 index 0000000000..6521e90b5c --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -0,0 +1,63 @@ +name: Rolling Semi-Binary Build +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + +jobs: + semi_binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..02ed08578b --- /dev/null +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -0,0 +1,58 @@ +name: Rolling Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.rolling.repos' + - '**.xml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: true diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 40abcd1b0c..e409150974 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -1,19 +1,29 @@ name: Rolling Source Build on: workflow_dispatch: - branches: - - master push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' jobs: source: - uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: rolling ref: master ros2_repo_branch: rolling + container: ubuntu:24.04 diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml new file mode 100644 index 0000000000..434344fcaa --- /dev/null +++ b/.github/workflows/rosdoc2.yml @@ -0,0 +1,17 @@ +name: rosdoc2 + +on: + workflow_dispatch: + pull_request: + paths: + - ros2_controllers/doc/** + - ros2_controllers/rosdoc2.yaml + - ros2_controllers/package.xml + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + check: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rosdoc2.yml@master diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml new file mode 100644 index 0000000000..6bedaa0c97 --- /dev/null +++ b/.github/workflows/update-pre-commit.yml @@ -0,0 +1,12 @@ +name: Auto Update pre-commit +# Update pre-commit config and create PR if changes are detected +# author: Christoph Fröhlich + +on: + workflow_dispatch: + schedule: + - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month + +jobs: + auto_update_and_create_pr: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..f26943c89e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,3 +1,4 @@ + # To use: # # pre-commit run -a @@ -15,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -25,72 +26,62 @@ repos: - id: check-symlinks - id: check-xml - id: check-yaml + args: ["--allow-multiple-documents"] - id: debug-statements - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace + exclude_types: [rst] - id: fix-byte-order-marker + # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.37.3 + rev: v3.19.1 hooks: - id: pyupgrade args: [--py36-plus] + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.3.0 + hooks: + - id: pydocstyle + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 25.1.0 hooks: - id: black args: ["--line-length=99"] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 - hooks: - - id: pep257 - args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - - repo: https://github.com/pycqa/flake8 - rev: 5.0.4 + rev: 7.2.0 hooks: - id: flake8 - args: ["--ignore=E501"] + args: ["--extend-ignore=E501"] # CPP hooks - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v20.1.0 hooks: - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format-12 - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] - # The same options as in ament_cppcheck are used, but its not working... - #- repo: https://github.com/pocc/pre-commit-hooks - #rev: v1.1.1 - #hooks: - #- id: cppcheck - #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] - repo: local hooks: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # Maybe use https://github.com/cpplint/cpplint instead - repo: local hooks: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -102,7 +93,6 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] entry: ament_lint_cmake language: system files: CMakeLists\.txt$ @@ -113,20 +103,20 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] entry: ament_copyright language: system + exclude: .*/conf\.py$ # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.0.0 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.9.0 + rev: v1.10.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ @@ -136,8 +126,18 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.4.1 hooks: - id: codespell - args: ['--write-changes', '--uri-ignore-words-list=ist'] + args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ + + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.32.1 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] diff --git a/README.md b/README.md index 6a59d0054e..f5f129e077 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,17 @@ # ros2_controllers -Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Navigation2. +[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/branch/humble/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers/tree/humble) + +Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Nav2. ## Build status ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Jazzy** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#jazzy) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) ### Explanation of different build types diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..970f9fd60e --- /dev/null +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -0,0 +1,236 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ackermann_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Fix typos in steering_controllers_lib (backport `#1464 `_) (`#1466 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* 🚀 Add PID controller 🎉 (backport `#434 `_, `#975 `_, `#899 `_, `#1084 `_, `#951 `_) (`#1162 `_) +* Fix steering controllers library kinematics (`#1150 `_) (`#1194 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- +* Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) +* Contributors: mergify[bot] + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- +* Bump versions for release +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* Fix sphinx for steering odometry library/controllers (`#626 `_) (`#661 `_) +* Steering odometry library and controllers (backport `#484 `_) (`#624 + $) +target_link_libraries(ackermann_steering_controller PUBLIC + ackermann_steering_controller_parameters) +ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ackermann_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + + add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller + test/test_load_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_ackermann_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_ackermann_steering_controller + test/test_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/ackermann_steering_controller +) + +install( + TARGETS ackermann_steering_controller ackermann_steering_controller_parameters + EXPORT export_ackermann_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_ackermann_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/ackermann_steering_controller/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml new file mode 100644 index 0000000000..2ac2150dd1 --- /dev/null +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints. + + + diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..daf4561861 --- /dev/null +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/ackermann_steering_controller/doc/userdoc.rst + +.. _ackermann_steering_controller_userdoc: + +ackermann_steering_controller +============================= + +This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +Additionally to the parameters of the :ref:`Steering Controller Library `, this controller adds the following parameters: + +.. generate_parameter_library_details:: ../src/ackermann_steering_controller.yaml diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp new file mode 100644 index 0000000000..c69d08809f --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include + +#include "ackermann_steering_controller/ackermann_steering_controller_parameters.hpp" +#include "ackermann_steering_controller/visibility_control.h" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace ackermann_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + AckermannSteeringController(); + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; + +protected: + std::shared_ptr ackermann_param_listener_; + ackermann_steering_controller::Params ackermann_params_; +}; +} // namespace ackermann_steering_controller + +#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h new file mode 100644 index 0000000000..177f0bf87c --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml new file mode 100644 index 0000000000..ab1aa814d4 --- /dev/null +++ b/ackermann_steering_controller/package.xml @@ -0,0 +1,45 @@ + + + + ackermann_steering_controller + 2.44.0 + Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + backward_ros + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface_testing + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp new file mode 100644 index 0000000000..77ba55812a --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" + +namespace ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void AckermannSteeringController::initialize_implementation_parameter_listener() +{ + ackermann_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() +{ + ackermann_params_ = ackermann_param_listener_->get_params(); + + const double front_wheels_radius = ackermann_params_.front_wheels_radius; + const double rear_wheels_radius = ackermann_params_.rear_wheels_radius; + const double front_wheel_track = ackermann_params_.front_wheel_track; + const double rear_wheel_track = ackermann_params_.rear_wheel_track; + const double wheelbase = ackermann_params_.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "Ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + if ( + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); + } + } + } + return true; +} +} // namespace ackermann_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ackermann_steering_controller::AckermannSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml new file mode 100644 index 0000000000..1ec0b41c9f --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -0,0 +1,55 @@ +ackermann_steering_controller: + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + rear_wheel_track: + { + type: double, + default_value: 0.0, + description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml new file mode 100644 index 0000000000..6b64f901c3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ecb1b071e3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint] + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..de45accc0a --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -0,0 +1,298 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); +} + +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(AckermannSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(AckermannSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AckermannSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + EXPECT_EQ(msg.steering_angle_command[1], 4.4); + + publish_commands(); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[1], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp new file mode 100644 index 0000000000..555009131f --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -0,0 +1,322 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL; +using ackermann_steering_controller::STATE_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using ackermann_steering_controller::CMD_STEER_LEFT_WHEEL; +using ackermann_steering_controller::CMD_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace + +// subclassing and friending so we can access member variables +class TestableAckermannSteeringController +: public ackermann_steering_controller::AckermannSteeringController +{ + FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_interfaces); + FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, update_success); + FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); + FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return ackermann_steering_controller::AckermannSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class AckermannSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..1a16bed838 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -0,0 +1,109 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); +} + +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..0a8cd7b80c --- /dev/null +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAckermannSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index cc7fa5f2bf..00d1e39619 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,163 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- +* Cleanup dependencies (add `angles`, rm `filters`) (backport `#1555 `_) (`#1556 `_) +* Contributors: mergify[bot] + +2.42.0 (2025-02-17) +------------------- +* [Admittance] multiple state/command interfaces (backport `#547 `_, `#1196 `_) (`#1548 `_) +* Remove dependency of admittance controller on JTC (backport `#1532 `_) (`#1537 `_) +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich, Marco Magri, Sai Kishor Kothakota + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- +* Fix segfault at reconfigure of AdmittanceController (`#1248 `_) (`#1265 `_) +* Contributors: mergify[bot] + +2.37.1 (2024-08-14) +------------------- +* Fix admittance controller interface read/write logic (backport `#1232 `_) (`#1234 `_) +* Contributors: mergify[bot] + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* Use CMake target for eigen (`#1058 `_) (`#1066 `_) +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* Update docs for diff drive controller (`#751 `_) (`#753 `_) +* Contributors: Christoph Fröhlich + +2.24.0 (2023-08-07) +------------------- +* Activate AdmittanceControllerTestParameterizedInvalidParameters (`#711 `_) (`#733 `_) +* Fix file name for include guard (backport `#681 `_) +* Fix out of bound access in admittance controller (`#721 `_) (`#722 `_) +* Contributors: Christoph Fröhlich + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* [JTC] Fix missing parameter deprecation warnings (`#630 `_) +* [Formatting] enable ReflowComments to also use ColumnLimit on comments (`#628 `_) +* Contributors: Noel Jiménez García, Sai Kishor Kothakota, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- +* Fix docs format (`#591 `_) +* Contributors: Christoph Fröhlich + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a6dcf35dce..39dfca8680 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -1,26 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(admittance_controller) +cmake_minimum_required(VERSION 3.16) +project(admittance_controller LANGUAGES CXX) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs control_toolbox controller_interface - kinematics_interface - Eigen3 generate_parameter_library geometry_msgs hardware_interface - joint_trajectory_controller + kinematics_interface pluginlib rclcpp rclcpp_lifecycle @@ -34,15 +27,29 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(Eigen3 REQUIRED NO_MODULE) -add_library(admittance_controller SHARED src/admittance_controller.cpp) -target_include_directories(admittance_controller PRIVATE include) -generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml) -target_link_libraries(admittance_controller admittance_controller_parameters) -ament_target_dependencies(admittance_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +generate_parameter_library(admittance_controller_parameters + src/admittance_controller_parameters.yaml +) + +add_library(admittance_controller SHARED + src/admittance_controller.cpp +) +target_compile_features(admittance_controller PUBLIC cxx_std_17) +target_include_directories(admittance_controller PUBLIC + $ + $ +) +target_link_libraries(admittance_controller PUBLIC + admittance_controller_parameters + Eigen3::Eigen +) +ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -50,43 +57,32 @@ target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_ pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS admittance_controller admittance_controller_parameters - EXPORT export_admittance_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(control_msgs REQUIRED) find_package(controller_manager REQUIRED) - find_package(controller_interface REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) + # Dynamically loaded during test + find_package(kinematics_interface_kdl REQUIRED) + # test loading admittance controller - add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) - target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) - ament_target_dependencies( - test_load_admittance_controller + add_rostest_with_parameters_gmock(test_load_admittance_controller + test/test_load_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml + ) + ament_target_dependencies(test_load_admittance_controller controller_manager hardware_interface ros2_control_test_assets ) + # test admittance controller function - add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_admittance_controller PRIVATE include) + add_rostest_with_parameters_gmock(test_admittance_controller + test/test_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml + ) target_link_libraries(test_admittance_controller admittance_controller) - ament_target_dependencies( - test_admittance_controller + ament_target_dependencies(test_admittance_controller control_msgs controller_interface hardware_interface @@ -94,10 +90,18 @@ if(BUILD_TESTING) ) endif() -ament_export_targets( - export_admittance_controller HAS_LIBRARY_TARGET +install( + DIRECTORY include/ + DESTINATION include/admittance_controller ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} + +install(TARGETS admittance_controller admittance_controller_parameters + EXPORT export_admittance_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_admittance_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 60837457f9..8056a017d7 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -1,4 +1,6 @@ -.. _joint_trajectory_controller_userdoc: +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/admittance_controller/doc/userdoc.rst + +.. _admittance_controller_userdoc: Admittance Controller ====================== @@ -8,20 +10,21 @@ The controller implements ``ChainedControllerInterface``, so it is possible to a The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. -Parameters: - - -ROS2 interface of the controller +ROS 2 interface of the controller --------------------------------- Parameters ^^^^^^^^^^^ -The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. -The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -An example parameter file can be found in the `test folder of the controller `_ +The admittance controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: ../src/admittance_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test folder `_: +.. literalinclude:: ../test/test_params.yaml + :language: yaml Topics ^^^^^^^ @@ -45,14 +48,14 @@ The controller has ``position`` and ``velocity`` reference interfaces exported i States ^^^^^^^ The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. If some interface is not provided, the last commanded interface will be used for calculation. -For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. Commands ^^^^^^^^^ The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index f776646d42..2de3b6541d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -22,8 +22,8 @@ #include #include -// include generated parameter library -#include "admittance_controller_parameters.hpp" +// auto-generated by generate_parameter_library +#include "admittance_controller/admittance_controller_parameters.hpp" #include "admittance_controller/admittance_rule.hpp" #include "admittance_controller/visibility_control.h" @@ -38,8 +38,8 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" @@ -126,8 +126,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt hardware_interface::HW_IF_ACCELERATION}; // internal reference values - const std::vector allowed_reference_interfaces_types_ = { - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; std::vector> position_reference_; std::vector> velocity_reference_; @@ -165,20 +163,22 @@ class AdmittanceController : public controller_interface::ChainableControllerInt geometry_msgs::msg::Wrench ft_values_; /** - * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values - */ + * @brief Read values from hardware interfaces and set corresponding fields of state_current and + * ft_values + */ void read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint & state_current, geometry_msgs::msg::Wrench & ft_values); /** - * @brief Set fields of state_reference with values from controllers exported position and velocity references - */ + * @brief Set fields of state_reference with values from controllers exported position and + * velocity references + */ void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); /** -* @brief Write values from state_command to claimed hardware interfaces -*/ + * @brief Write values from state_command to claimed hardware interfaces + */ void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f0aabb063..2b6d7a9459 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -24,6 +24,7 @@ #include #include +#include "admittance_controller/admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "control_toolbox/filters.hpp" #include "controller_interface/controller_interface.hpp" @@ -43,13 +44,17 @@ struct AdmittanceTransforms { // transformation from force torque sensor frame to base link frame at reference joint angles Eigen::Isometry3d ref_base_ft_; - // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + // transformation from force torque sensor frame to base link frame at reference + admittance + // offset joint angles Eigen::Isometry3d base_ft_; - // transformation from control frame to base link frame at reference + admittance offset joint angles + // transformation from control frame to base link frame at reference + admittance offset joint + // angles Eigen::Isometry3d base_control_; - // transformation from end effector frame to base link frame at reference + admittance offset joint angles + // transformation from end effector frame to base link frame at reference + admittance offset + // joint angles Eigen::Isometry3d base_tip_; - // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles + // transformation from center of gravity frame to base link frame at reference + admittance offset + // joint angles Eigen::Isometry3d base_cog_; // transformation from world frame to base link frame Eigen::Isometry3d world_base_; @@ -66,10 +71,11 @@ struct AdmittanceState mass_inv.setZero(); stiffness.setZero(); selected_axes.setZero(); - current_joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_vel = Eigen::VectorXd::Zero(num_joints); - joint_acc = Eigen::VectorXd::Zero(num_joints); + auto idx = static_cast(num_joints); + current_joint_pos = Eigen::VectorXd::Zero(idx); + joint_pos = Eigen::VectorXd::Zero(idx); + joint_vel = Eigen::VectorXd::Zero(idx); + joint_acc = Eigen::VectorXd::Zero(idx); } Eigen::VectorXd current_joint_pos; @@ -111,20 +117,20 @@ class AdmittanceRule controller_interface::return_type reset(const size_t num_joints); /** - * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does - * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation - * are calculated without an error - * \param[in] current_joint_state current joint state of the robot - * \param[in] reference_joint_state input joint state reference - * \param[out] success true if no calls to the kinematics interface fail + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If + * the transform does not exist in the kinematics model, then TF will be used for lookup. The + * return value is true if all transformation are calculated without an error \param[in] + * current_joint_state current joint state of the robot \param[in] reference_joint_state input + * joint state reference \param[out] success true if no calls to the kinematics interface fail */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** - * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field - * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent + * Eigen field members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, + * damping_) are also updated */ void apply_parameters_update(); @@ -136,7 +142,8 @@ class AdmittanceRule * \param[in] measured_wrench most recent measured wrench from force torque sensor * \param[in] reference_joint_state input joint state reference * \param[in] period time in seconds since last controller update - * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to + * the input reference */ controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, @@ -148,7 +155,8 @@ class AdmittanceRule /** * Set fields of `state_message` from current admittance controller state. * - * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things + * \param[out] state_message message containing target position/vel/accel, wrench, and actual + * robot state, among other things */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); @@ -159,22 +167,21 @@ class AdmittanceRule protected: /** - * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input - * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin - * calls failed. - * \param[in] admittance_state contains all the information needed to calculate the admittance offset - * \param[in] dt controller period + * Calculates the admittance rule from given the robot's current joint angles. The admittance + * controller state input is updated with the new calculated values. A boolean value is returned + * indicating if any of the kinematics plugin calls failed. \param[in] admittance_state contains + * all the information needed to calculate the admittance offset \param[in] dt controller period * \param[out] success true if no calls to the kinematics interface fail */ bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, - * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. - * The `wrench_world_` estimate includes gravity compensation - * \param[in] measured_wrench most recent measured wrench from force torque sensor - * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame - * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement + * `measured_wrench`, the sensor to base frame rotation `sensor_world_rot`, and the center of + * gravity frame to base frame rotation `cog_world_rot`. The `wrench_world_` estimate includes + * gravity compensation \param[in] measured_wrench most recent measured wrench from force torque + * sensor \param[in] sensor_world_rot rotation matrix from world frame to sensor frame \param[in] + * cog_world_rot rotation matrix from world frame to center of gravity frame */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 87c16e4787..4f270304f0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -28,6 +28,9 @@ namespace admittance_controller { + +constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) + /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( const std::shared_ptr & node, const size_t num_joints) @@ -42,11 +45,17 @@ controller_interface::return_type AdmittanceRule::configure( { try { + // Make sure we destroy the interface first. Otherwise we might run into a segfault + if (kinematics_loader_) + { + kinematics_.reset(); + } kinematics_loader_ = std::make_shared>( parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( node->get_node_parameters_interface(), parameters_.kinematics.tip)) { @@ -83,10 +92,10 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) { state_message_.joint_state.name = parameters_.joints; } - state_message_.mass.data.resize(6, 0.0); - state_message_.selected_axes.data.resize(6, 0); - state_message_.damping.data.resize(6, 0); - state_message_.stiffness.data.resize(6, 0); + state_message_.mass.data.resize(NUM_CARTESIAN_DOF, 0.0); + state_message_.selected_axes.data.resize(NUM_CARTESIAN_DOF, 0); + state_message_.damping.data.resize(NUM_CARTESIAN_DOF, 0); + state_message_.stiffness.data.resize(NUM_CARTESIAN_DOF, 0); state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; @@ -122,11 +131,13 @@ void AdmittanceRule::apply_parameters_update() vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); - for (size_t i = 0; i < 6; ++i) + for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * - sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + auto idx = static_cast(i); + admittance_state_.mass_inv[idx] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[idx] = + parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[idx] * admittance_state_.stiffness[idx]); } } @@ -204,12 +215,13 @@ controller_interface::return_type AdmittanceRule::update( // update joint desired joint state for (size_t i = 0; i < num_joints_; ++i) { + auto idx = static_cast(i); desired_joint_state.positions[i] = - reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + reference_joint_state.positions[i] + admittance_state_.joint_pos[idx]; desired_joint_state.velocities[i] = - reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + reference_joint_state.velocities[i] + admittance_state_.joint_vel[idx]; desired_joint_state.accelerations[i] = - reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[idx]; } return controller_interface::return_type::OK; @@ -322,7 +334,7 @@ void AdmittanceRule::process_wrench_measurements( new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); // apply smoothing filter - for (size_t i = 0; i < 6; ++i) + for (Eigen::Index i = 0; i < 6; ++i) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); @@ -331,16 +343,22 @@ void AdmittanceRule::process_wrench_measurements( const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() { + for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) + { + auto idx = static_cast(i); + state_message_.stiffness.data[i] = admittance_state_.stiffness[idx]; + state_message_.damping.data[i] = admittance_state_.damping[idx]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[idx]); + state_message_.mass.data[i] = admittance_state_.mass[idx]; + } + for (size_t i = 0; i < parameters_.joints.size(); ++i) { + auto idx = static_cast(i); state_message_.joint_state.name[i] = parameters_.joints[i]; - state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; - state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; - state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - state_message_.damping.data[i] = admittance_state_.damping[i]; - state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - state_message_.mass.data[i] = admittance_state_.mass[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[idx]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[idx]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[idx]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; @@ -395,7 +413,7 @@ void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) { for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col * matrix.rows()]; + matrix(row, col) = data[static_cast(row + col * matrix.rows())]; } } } diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 876ed424b0..7f0fef0288 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,39 +2,49 @@ admittance_controller - 2.15.0 + 2.44.0 Implementation of admittance controllers for different input and output interface. - Denis Štogl + Bence Magyar - Andy Zelenak + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Andy Zelenak + Paul Gesel + ament_cmake + angles + backward_ros control_msgs control_toolbox controller_interface - kinematics_interface - filters generate_parameter_library geometry_msgs hardware_interface - joint_trajectory_controller + kinematics_interface pluginlib - rclcpp rclcpp_lifecycle + rclcpp realtime_tools - tf2 tf2_eigen tf2_geometry_msgs tf2_kdl tf2_ros + tf2 trajectory_msgs ament_cmake_gmock - control_msgs controller_manager - hardware_interface + hardware_interface_testing kinematics_interface_kdl ros2_control_test_assets diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1af4e1317f..f9978b4ecf 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -123,7 +123,7 @@ AdmittanceController::on_export_reference_interfaces() // assign reference interfaces auto index = 0ul; - for (const auto & interface : allowed_reference_interfaces_types_) + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { for (const auto & joint : admittance_->parameters_.joints) { @@ -134,8 +134,9 @@ AdmittanceController::on_export_reference_interfaces() velocity_reference_.emplace_back(reference_interfaces_[index]); } const auto full_name = joint + "/" + interface; - chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( - std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + chainable_command_interfaces.emplace_back( + hardware_interface::CommandInterface( + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); index++; } @@ -147,18 +148,6 @@ AdmittanceController::on_export_reference_interfaces() controller_interface::CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - try - { - parameter_handler_ = std::make_shared(get_node()); - admittance_ = std::make_unique(parameter_handler_); - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - command_joint_names_ = admittance_->parameters_.command_joints; if (command_joint_names_.empty()) { @@ -256,6 +245,12 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( has_acceleration_state_interface_ = contains_interface_type( admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + if (!has_position_state_interface_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Position state interface is required."); + return CallbackReturn::FAILURE; + } + auto get_interface_list = [](const std::vector & interface_types) { std::stringstream ss_command_interfaces; @@ -318,7 +313,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, admittance_->parameters_.joints, interface, joint_state_interface_[index])) @@ -333,7 +328,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { @@ -383,13 +378,22 @@ controller_interface::return_type AdmittanceController::update_reference_from_su // if message exists, load values into references if (joint_command_msg_.get()) { - for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { - position_reference_[i].get() = joint_command_msg_->positions[i]; - } - for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) - { - velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + if (interface == hardware_interface::HW_IF_POSITION) + { + for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + { + position_reference_[i].get() = joint_command_msg_->positions[i]; + } + } + else if (interface == hardware_interface::HW_IF_VELOCITY) + { + for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) + { + velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + } + } } } @@ -439,8 +443,13 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( // reset to prevent stale references for (size_t i = 0; i < num_joints_; i++) { - position_reference_[i].get() = std::numeric_limits::quiet_NaN(); - velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) + { + if (interface == hardware_interface::HW_IF_POSITION) + position_reference_[i].get() = std::numeric_limits::quiet_NaN(); + else if (interface == hardware_interface::HW_IF_VELOCITY) + velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + } } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -481,7 +490,7 @@ void AdmittanceController::read_state_from_hardware( bool nan_acceleration = false; size_t pos_ind = 0; - size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t vel_ind = pos_ind + has_velocity_state_interface_; size_t acc_ind = vel_ind + has_acceleration_state_interface_; for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { @@ -491,13 +500,13 @@ void AdmittanceController::read_state_from_hardware( state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); nan_position |= std::isnan(state_current.positions[joint_ind]); } - else if (has_velocity_state_interface_) + if (has_velocity_state_interface_) { state_current.velocities[joint_ind] = state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); nan_velocity |= std::isnan(state_current.velocities[joint_ind]); } - else if (has_acceleration_state_interface_) + if (has_acceleration_state_interface_) { state_current.accelerations[joint_ind] = state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); @@ -534,8 +543,9 @@ void AdmittanceController::write_state_to_hardware( { // if any interface has nan values, assume state_commanded is the last command state size_t pos_ind = 0; - size_t vel_ind = pos_ind + has_velocity_command_interface_; - size_t acc_ind = vel_ind + has_acceleration_state_interface_; + size_t vel_ind = + (has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind; + size_t acc_ind = vel_ind + has_acceleration_command_interface_; for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { if (has_position_command_interface_) @@ -543,15 +553,15 @@ void AdmittanceController::write_state_to_hardware( command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( state_commanded.positions[joint_ind]); } - else if (has_velocity_command_interface_) + if (has_velocity_command_interface_) { command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.velocities[joint_ind]); } - else if (has_acceleration_command_interface_) + if (has_acceleration_command_interface_) { command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.accelerations[joint_ind]); } } last_commanded_ = state_commanded; @@ -565,19 +575,28 @@ void AdmittanceController::read_state_reference_interfaces( // if any interface has nan values, assume state_reference is the last set reference for (size_t i = 0; i < num_joints_; ++i) { - // update position - if (std::isnan(position_reference_[i])) + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { - position_reference_[i].get() = last_reference_.positions[i]; - } - state_reference.positions[i] = position_reference_[i]; + // update position + if (interface == hardware_interface::HW_IF_POSITION) + { + if (std::isnan(position_reference_[i])) + { + position_reference_[i].get() = last_reference_.positions[i]; + } + state_reference.positions[i] = position_reference_[i]; + } - // update velocity - if (std::isnan(velocity_reference_[i])) - { - velocity_reference_[i].get() = last_reference_.velocities[i]; + // update velocity + if (interface == hardware_interface::HW_IF_VELOCITY) + { + if (std::isnan(velocity_reference_[i])) + { + velocity_reference_[i].get() = last_reference_.velocities[i]; + } + state_reference.velocities[i] = velocity_reference_[i]; + } } - state_reference.velocities[i] = velocity_reference_[i]; } last_reference_.positions = state_reference.positions; diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 5bdc40e398..bbd81d3553 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -27,6 +27,7 @@ admittance_controller: chainable_command_interfaces: { type: string_array, + default_value: ["position", "velocity"], description: "Specifies which reference interfaces the controller will export. Normally, the position and velocity are used.", read_only: true } @@ -142,7 +143,7 @@ admittance_controller: description: "Specifies the joint damping applied used in the admittance calculation.", default_value: 5, validation: { - lower_bounds: [ 0.0 ] + gt_eq: [ 0.0 ] } } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index d056e0406c..07b64f4834 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -21,20 +21,17 @@ #include #include -// Test on_configure returns ERROR when a required parameter is missing -TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +// Test on_init returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_init_parameter_is_missing) { ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } INSTANTIATE_TEST_SUITE_P( - MissingMandatoryParameterDuringConfiguration, - AdmittanceControllerTestParameterizedMissingParameters, + MissingMandatoryParameterDuringInit, AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( - "admittance.mass", "admittance.selected_axes", "admittance.stiffness", - "chainable_command_interfaces", "command_interfaces", "control.frame.id", - "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", + "admittance.mass", "admittance.selected_axes", "admittance.stiffness", "command_interfaces", + "control.frame.id", "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); @@ -67,10 +64,18 @@ INSTANTIATE_TEST_SUITE_P( // wrong length selected axes std::make_tuple( std::string("admittance.selected_axes"), - rclcpp::ParameterValue(std::vector() = {1, 2, 3})), - // invalid robot description - std::make_tuple( - std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); + rclcpp::ParameterValue(std::vector() = {1, 2, 3})) + // invalid robot description. + // TODO(anyone): deactivated, because SetUpController returns SUCCESS here? + // ,std::make_tuple( + // std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))) + )); + +// Test on_init returns ERROR when a parameter is invalid +TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, invalid_parameters) +{ + ASSERT_EQ(SetUpController(), controller_interface::return_type::ERROR); +} TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { @@ -82,26 +87,30 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.joints.begin(), - controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.joints.begin(), + controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), + joint_names_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.command_interfaces.empty()); ASSERT_TRUE( controller_->admittance_->parameters_.command_interfaces.size() == command_interface_types_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.command_interfaces.begin(), - controller_->admittance_->parameters_.command_interfaces.end(), - command_interface_types_.begin(), command_interface_types_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.command_interfaces.begin(), + controller_->admittance_->parameters_.command_interfaces.end(), + command_interface_types_.begin(), command_interface_types_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.state_interfaces.empty()); ASSERT_TRUE( controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.state_interfaces.begin(), - controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), - state_interface_types_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.state_interfaces.begin(), + controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), + state_interface_types_.end())); ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); @@ -111,36 +120,40 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_TRUE( controller_->admittance_->parameters_.admittance.selected_axes.size() == admittance_selected_axes_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.admittance.selected_axes.begin(), - controller_->admittance_->parameters_.admittance.selected_axes.end(), - admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.admittance.selected_axes.begin(), + controller_->admittance_->parameters_.admittance.selected_axes.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.mass.empty()); ASSERT_TRUE( controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.admittance.mass.begin(), - controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), - admittance_mass_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.admittance.mass.begin(), + controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), + admittance_mass_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.damping_ratio.empty()); ASSERT_TRUE( controller_->admittance_->parameters_.admittance.damping_ratio.size() == admittance_damping_ratio_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.admittance.damping_ratio.begin(), - controller_->admittance_->parameters_.admittance.damping_ratio.end(), - admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.admittance.damping_ratio.begin(), + controller_->admittance_->parameters_.admittance.damping_ratio.end(), + admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.stiffness.empty()); ASSERT_TRUE( controller_->admittance_->parameters_.admittance.stiffness.size() == admittance_stiffness_.size()); - ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.admittance.stiffness.begin(), - controller_->admittance_->parameters_.admittance.stiffness.end(), admittance_stiffness_.begin(), - admittance_stiffness_.end())); + ASSERT_TRUE( + std::equal( + controller_->admittance_->parameters_.admittance.stiffness.begin(), + controller_->admittance_->parameters_.admittance.stiffness.end(), + admittance_stiffness_.begin(), admittance_stiffness_.end())); } TEST_F(AdmittanceControllerTest, check_interfaces) @@ -151,12 +164,15 @@ TEST_F(AdmittanceControllerTest, check_interfaces) auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), @@ -173,6 +189,49 @@ TEST_F(AdmittanceControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); } +TEST_F(AdmittanceControllerTest, missing_pos_state_interface) +{ + auto overrides = {rclcpp::Parameter("state_interfaces", std::vector{"velocity"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); +} + +TEST_F(AdmittanceControllerTest, only_vel_command_interface) +{ + command_interface_types_ = {"velocity"}; + auto overrides = {rclcpp::Parameter("command_interfaces", std::vector{"velocity"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, only_pos_reference_interface) +{ + auto overrides = { + rclcpp::Parameter("chainable_command_interfaces", std::vector{"position"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, only_vel_reference_interface) +{ + auto overrides = { + rclcpp::Parameter("chainable_command_interfaces", std::vector{"velocity"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, invalid_reference_interface) +{ + auto overrides = {rclcpp::Parameter( + "chainable_command_interfaces", std::vector{"invalid_interface"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + TEST_F(AdmittanceControllerTest, update_success) { SetUpController(); @@ -268,7 +327,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); broadcast_tfs(); ASSERT_EQ( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index be78f05bb9..75ac82689d 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -27,7 +27,6 @@ #include "gmock/gmock.h" -#include "6d_robot_description.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +37,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" +#include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" @@ -53,17 +53,10 @@ const double COMMON_THRESHOLD = 0.001; constexpr auto NODE_SUCCESS = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_FAILURE = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} - } // namespace // subclassing and friending so we can access member variables @@ -90,39 +83,27 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); - } - return ret; + return admittance_controller::AdmittanceController::on_configure(previous_state); } /** * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = - input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } private: - rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; - rclcpp::WaitSet input_pose_command_subscriber_wait_set_; const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; @@ -199,8 +180,9 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_command_values_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } @@ -214,8 +196,9 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < joint_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_itfs_.emplace_back( + hardware_interface::StateInterface( + joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -224,8 +207,9 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_itfs_.emplace_back( + hardware_interface::StateInterface( + ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -277,9 +261,12 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + ControllerStateMsg::SharedPtr received_msg; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/status", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node_->get_node_base_interface()); // call update to publish the test value ASSERT_EQ( @@ -287,11 +274,18 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node_->get_clock()->now() + timeout; + while (!received_msg && test_subscription_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands() @@ -367,7 +361,7 @@ class AdmittanceControllerTest : public ::testing::Test // Controller-related parameters const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; - const std::vector command_interface_types_ = {"position"}; + std::vector command_interface_types_ = {"position"}; const std::vector state_interface_types_ = {"position"}; const std::string ft_sensor_name_ = "ft_sensor_name"; @@ -454,9 +448,15 @@ class AdmittanceControllerTestParameterizedInvalidParameters static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } protected: - void SetUpController() + controller_interface::return_type SetUpController() { - AdmittanceControllerTest::SetUpController("test_admittance_controller"); + auto param_name = std::get<0>(GetParam()); + auto param_value = std::get<1>(GetParam()); + std::vector parameter_overrides; + rclcpp::Parameter param(param_name, param_value); + parameter_overrides.push_back(param); + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller", parameter_overrides); } }; diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp similarity index 98% rename from admittance_controller/test/6d_robot_description.hpp rename to admittance_controller/test/test_asset_6d_robot_description.hpp index f67b5bd054..4d38df7c30 100644 --- a/admittance_controller/test/6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ -#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ +#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #include @@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf = } // namespace ros2_control_test_assets -#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#endif // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 1f290aeff6..23be1f23f5 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -33,8 +33,9 @@ TEST(TestLoadAdmittanceController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController")); + ASSERT_EQ( + cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), + nullptr); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..c854f8d007 --- /dev/null +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -0,0 +1,237 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package bicycle_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Fix typos in steering_controllers_lib (backport `#1464 `_) (`#1466 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* 🚀 Add PID controller 🎉 (backport `#434 `_, `#975 `_, `#899 `_, `#1084 `_, `#951 `_) (`#1162 `_) +* Fix steering controllers library kinematics (`#1150 `_) (`#1194 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- +* Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) +* Contributors: mergify[bot] + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +* Bump versions for release +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* Fix sphinx for steering odometry library/controllers (`#626 `_) (`#661 `_) +* Steering odometry library and controllers (backport `#484 `_) (`#624 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković, Reza Kermani, Denis Štogl + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..5d662c1fec --- /dev/null +++ b/bicycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(bicycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(bicycle_steering_controller_parameters + src/bicycle_steering_controller.yaml +) + +add_library( + bicycle_steering_controller + SHARED + src/bicycle_steering_controller.cpp +) +target_compile_features(bicycle_steering_controller PUBLIC cxx_std_17) +target_include_directories(bicycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(bicycle_steering_controller PUBLIC + bicycle_steering_controller_parameters) +ament_target_dependencies(bicycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(bicycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface bicycle_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller + test/test_load_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_bicycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller test/test_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml) + target_include_directories(test_bicycle_steering_controller PRIVATE include) + target_link_libraries(test_bicycle_steering_controller bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/bicycle_steering_controller +) + +install( + TARGETS bicycle_steering_controller bicycle_steering_controller_parameters + EXPORT export_bicycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_bicycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/bicycle_steering_controller/bicycle_steering_controller.xml b/bicycle_steering_controller/bicycle_steering_controller.xml new file mode 100644 index 0000000000..644c8840fa --- /dev/null +++ b/bicycle_steering_controller/bicycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Bicycle kinematics with one traction and one steering joint. + + + diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..6fccaf7b73 --- /dev/null +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,25 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/bicycle_steering_controller/doc/userdoc.rst + +.. _bicycle_steering_controller_userdoc: + +bicycle_steering_controller +============================= + +This controller implements the kinematics with two axes and two wheels, where the wheel on one axis is fixed (traction/drive), and the wheel on the other axis is steerable. + +The controller expects to have one commanding joint for traction, and one commanding joint for steering. +If your Ackermann steering vehicle uses differentials on axes, then you should probably use this controller since you can command only one traction velocity and steering angle for virtual wheels in the middle of the axes. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +Additionally to the parameters of the :ref:`Steering Controller Library `, this controller adds the following parameters: + +.. generate_parameter_library_details:: ../src/bicycle_steering_controller.yaml diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp new file mode 100644 index 0000000000..bb72922213 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ +#define BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "bicycle_steering_controller/bicycle_steering_controller_parameters.hpp" +#include "bicycle_steering_controller/visibility_control.h" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace bicycle_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_WHEEL = 0; +static constexpr size_t STATE_STEER_AXIS = 1; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_WHEEL = 0; +static constexpr size_t CMD_STEER_WHEEL = 1; + +static constexpr size_t NR_STATE_ITFS = 2; +static constexpr size_t NR_CMD_ITFS = 2; +static constexpr size_t NR_REF_ITFS = 2; + +class BicycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + BicycleSteeringController(); + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +protected: + std::shared_ptr bicycle_param_listener_; + bicycle_steering_controller::Params bicycle_params_; +}; +} // namespace bicycle_steering_controller + +#endif // BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..b076a00215 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml new file mode 100644 index 0000000000..111f6deb20 --- /dev/null +++ b/bicycle_steering_controller/package.xml @@ -0,0 +1,45 @@ + + + + bicycle_steering_controller + 2.44.0 + Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + backward_ros + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp new file mode 100644 index 0000000000..95eaf1965c --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" + +namespace bicycle_steering_controller +{ +BicycleSteeringController::BicycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void BicycleSteeringController::initialize_implementation_parameter_listener() +{ + bicycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() +{ + bicycle_params_ = bicycle_param_listener_->get_params(); + + const double wheelbase = bicycle_params_.wheelbase; + const double front_wheel_radius = bicycle_params_.front_wheel_radius; + const double rear_wheel_radius = bicycle_params_.rear_wheel_radius; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + } + else + { + odometry_.set_wheel_params(front_wheel_radius, wheelbase); + } + + odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "bicycle odometry configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds()); + } + } + } + return true; +} +} // namespace bicycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + bicycle_steering_controller::BicycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml new file mode 100644 index 0000000000..fde323ef74 --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -0,0 +1,33 @@ +bicycle_steering_controller: + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + front_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Front wheel radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + rear_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheel radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml new file mode 100644 index 0000000000..a2a6c6508b --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -0,0 +1,15 @@ +test_bicycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..39ffeed878 --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_bicycle_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..a06aabbd20 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -0,0 +1,266 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); +} + +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(BicycleSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(BicycleSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(BicycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(BicycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(BicycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(BicycleSteeringControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); +} + +TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[0], 1.1); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(0.1, 0.2); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp new file mode 100644 index 0000000000..e9160d23d7 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -0,0 +1,288 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using bicycle_steering_controller::STATE_STEER_AXIS; +using bicycle_steering_controller::STATE_TRACTION_WHEEL; + +// name constants for command interfaces +using bicycle_steering_controller::CMD_STEER_WHEEL; +using bicycle_steering_controller::CMD_TRACTION_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; + +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableBicycleSteeringController +: public bicycle_steering_controller::BicycleSteeringController +{ + FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_interfaces); + FRIEND_TEST(BicycleSteeringControllerTest, activate_success); + FRIEND_TEST(BicycleSteeringControllerTest, update_success); + FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(BicycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(BicycleSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return bicycle_steering_controller::BicycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class BicycleSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_bicycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + + std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; + + double wheelbase_ = 3.24644; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {3.3, 0.5}; + std::array joint_command_values_ = {1.1, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::string steering_interface_name_ = "position"; + + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..bc3a182753 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); +} + +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..955feb33c5 --- /dev/null +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadBicycleSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/codecov.yml b/codecov.yml index 02deea2321..ca8c1cc0ac 100644 --- a/codecov.yml +++ b/codecov.yml @@ -9,6 +9,8 @@ coverage: patch: off fixes: - "ros_ws/src/ros2_controllers/::" +ignore: + - "**/test" comment: layout: "diff, flags, files" behavior: default diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3546af9bba..a79a8784ad 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,171 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Check dt in updateFromVelocity (backport `#1481 `_) (`#1486 `_) +* Remove empty on_shutdown() callbacks (backport `#1477 `_) (`#1482 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Improve tf_prefix based on namespace (`#1420 `_) (`#1421 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) +* Bump version of pre-commit hooks (`#1157 `_) (`#1158 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- +* Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) +* Contributors: mergify[bot] + +2.34.0 (2024-04-01) +------------------- +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- +* [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (backport `#533 `_) (`#726 `_) +* Contributors: mergify[bot] + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) (`#823 `_) +* Contributors: Tony Baltovski + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) (`#777 `_) +* Update docs for diff drive controller (`#751 `_) (`#753 `_) +* Contributors: Christoph Fröhlich, Jules CARPENTIER + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Use generate_parameter_library for all params (`#601 `_) (`#627 `_) +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* [Formatting] enable ReflowComments to also use ColumnLimit on comments (`#628 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix compilation warnings (`#621 `_) (`#623 `_) +* Remove compile warnings. (`#519 `_) (`#620 `_) +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich, Mathias Lüdtke, Noel Jiménez García + +2.20.0 (2023-05-14) +------------------- +* Clear registered handles of DiffDriveController on deactivate (`#596 `_) (`#606 `_) +* Contributors: Noel Jiménez García + +2.19.0 (2023-05-02) +------------------- +* Fix wrong publish timestamp initialization (`#585 `_) (`#593 `_) +* Contributors: mergify[bot] + +2.18.0 (2023-04-29) +------------------- +* adjusted open_loop param description in diff_drive_controller_parameter.yaml (`#570 `_) (`#576 `_) +* Contributors: muritane + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- +* [DiffDriveController] Fix prefixing of frame id with controller's namespace (`#522 `_) +* Contributors: Tim Verbelen + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* diff_drive base_frame_id param (`#495 `_) (`#498 `_) +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar, Jakub Delicat + 2.15.0 (2022-12-06) ------------------- * [DiffDriveController] Use generate parameter library (`#386 `_) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 83f18a9931..aa71c70745 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -1,81 +1,62 @@ -cmake_minimum_required(VERSION 3.5) -project(diff_drive_controller) +cmake_minimum_required(VERSION 3.16) +project(diff_drive_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) - generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml ) -add_library(${PROJECT_NAME} SHARED +add_library(diff_drive_controller SHARED src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${PROJECT_NAME} - diff_drive_controller_parameters +target_compile_features(diff_drive_controller PUBLIC cxx_std_17) +target_include_directories(diff_drive_controller PUBLIC + $ + $ ) +target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) +ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) - target_include_directories(test_diff_drive_controller PRIVATE include) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller - ${PROJECT_NAME} + diff_drive_controller ) - ament_target_dependencies(test_diff_drive_controller geometry_msgs hardware_interface @@ -87,25 +68,27 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock( - test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) - target_include_directories(test_load_diff_drive_controller PRIVATE include) ament_target_dependencies(test_load_diff_drive_controller controller_manager ros2_control_test_assets ) - endif() -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/diff_drive_controller ) -ament_export_libraries( - ${PROJECT_NAME} +install(TARGETS diff_drive_controller diff_drive_controller_parameters + EXPORT export_diff_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_diff_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/diff_drive_controller/doc/parameters_context.yaml b/diff_drive_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..81e92806f5 --- /dev/null +++ b/diff_drive_controller/doc/parameters_context.yaml @@ -0,0 +1,9 @@ +linear.x: | + Joint limits structure for the linear ``x``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z: | + Joint limits structure for the rotation about ``z``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 1ef47bd975..30fce79946 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -1,86 +1,80 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/diff_drive_controller/doc/userdoc.rst + .. _diff_drive_controller_userdoc: diff_drive_controller ===================== Controller for mobile robots with differential drive. -Input for control are robot body velocity commands which are translated to wheel commands for the differential drive base. -Odometry is computed from hardware feedback and published. -Velocity commands ------------------ +As input it takes velocity commands for the robot body, which are translated to wheel commands for the differential drive base. -The controller works with a velocity twist from which it extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. - -Hardware interface type ------------------------ +Odometry is computed from hardware feedback and published. -The controller works with wheel joints through a velocity interface. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. Other features -------------- - Realtime-safe implementation. - Odometry publishing - Task-space velocity, acceleration and jerk limits - Automatic stop after command time-out + + Realtime-safe implementation. + + Odometry publishing + + Task-space velocity, acceleration and jerk limits + + Automatic stop after command time-out -ros2_control Interfaces ------------------------- +Description of controller's interfaces +------------------------------------------------ References -,,,,,,,,,,, +,,,,,,,,,,,,,,,,,, +(the controller is not yet implemented as chainable controller) -States -,,,,,,, +Feedback +,,,,,,,,,,,,,, +As feedback interface type the joints' position (``hardware_interface::HW_IF_POSITION``) or velocity (``hardware_interface::HW_IF_VELOCITY``,if parameter ``position_feedback=false``) are used. -Commands +Output ,,,,,,,,, +Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used. -ROS2 Interfaces ----------------- + +ROS 2 Interfaces +------------------------ Subscribers ,,,,,,,,,,,, + ~/cmd_vel [geometry_msgs/msg/TwistStamped] - Velocity command for the controller. + Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. ~/cmd_vel_unstamped [geometry_msgs::msg::Twist] - -~/cmd_vel_out [] - - + Velocity command for the controller, if ``use_stamped_vel=false``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. Publishers ,,,,,,,,,,, -~/odom [] +~/odom [nav_msgs::msg::Odometry] + This represents an estimate of the robot's position and velocity in free space. -/tf +/tf [tf2_msgs::msg::TFMessage] + tf tree. Published only if ``enable_odom_tf=true`` - -Services -,,,,,,,,, +~/cmd_vel_out [geometry_msgs/msg/TwistStamped] + Velocity command for the controller, where limits were applied. Published only if ``publish_limited_velocity=true`` Parameters ------------- +,,,,,,,,,,,, -Check `parameter definition file for details `_. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -Note that the documentation on parameters for joint limits can be found in `their header file `_. -Those parameters are: +.. generate_parameter_library_details:: ../src/diff_drive_controller_parameter.yaml + parameters_context.yaml -linear.x [JointLimits structure] - Joint limits structure for the linear X-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +An example parameter file for this controller can be found in `the test directory `_: -angular.z [JointLimits structure] - Joint limits structure for the rotation about Z-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +.. literalinclude:: ../test/config/test_diff_drive_controller.yaml + :language: yaml diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index f229667fb9..2f35f24141 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -37,12 +37,12 @@ #include "odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "diff_drive_controller_parameters.hpp" +// auto-generated by generate_parameter_library +#include "diff_drive_controller/diff_drive_controller_parameters.hpp" namespace diff_drive_controller { @@ -87,10 +87,6 @@ class DiffDriveController : public controller_interface::ControllerInterface controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - protected: struct WheelHandle { @@ -147,7 +143,7 @@ class DiffDriveController : public controller_interface::ControllerInterface // publish rate limiter double publish_rate_ = 50.0; rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0}; + rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; bool is_halted = false; bool use_stamped_vel_ = true; diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8b2e368824..51b13d36f8 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,20 +1,34 @@ diff_drive_controller - 2.15.0 - Controller for a differential drive mobile base. + 2.44.0 + Controller for a differential-drive mobile base. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Bence Magyar + Enrique Fernández + Manuel Meraz + Jordan Palacios + ament_cmake generate_parameter_library + backward_ros controller_interface geometry_msgs hardware_interface nav_msgs + pluginlib rclcpp rclcpp_lifecycle rcpputils @@ -22,10 +36,9 @@ tf2 tf2_msgs - pluginlib - ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 75d8b1fa35..05fb52cb79 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -150,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < params_.wheels_per_side; ++index) + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= params_.wheels_per_side; - right_feedback_mean /= params_.wheels_per_side; + left_feedback_mean /= static_cast(params_.wheels_per_side); + right_feedback_mean /= static_cast(params_.wheels_per_side); if (params_.position_feedback) { @@ -185,10 +185,24 @@ controller_interface::return_type DiffDriveController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < time) + bool should_publish = false; + try + { + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + should_publish = true; + } + } + catch (const std::runtime_error &) { - previous_publish_timestamp_ += publish_period_; + // Handle exceptions when the time source changes and initialize publish timestamp + previous_publish_timestamp_ = time; + should_publish = true; + } + if (should_publish) + { if (realtime_odometry_publisher_->trylock()) { auto & odometry_message = realtime_odometry_publisher_->msg_; @@ -244,7 +258,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < params_.wheels_per_side; ++index) + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -284,12 +298,11 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); - cmd_vel_timeout_ = std::chrono::milliseconds{ - static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; + publish_limited_velocity_ = params_.publish_limited_velocity; + use_stamped_vel_ = params_.use_stamped_vel; limiter_linear_ = SpeedLimiter( params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, @@ -372,35 +385,47 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( }); } - // initialize odometry publisher and messasge + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = std::make_shared>( odometry_publisher_); - std::string controller_namespace = std::string(get_node()->get_namespace()); - - if (controller_namespace == "/") + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) { - controller_namespace = ""; - } - else - { - controller_namespace = controller_namespace + "/"; + if (params_.tf_frame_prefix != "") + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + } + + // Make sure prefix does not start with '/' and always ends with '/' + if (tf_prefix.back() != '/') + { + tf_prefix = tf_prefix + "/"; + } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } } - const auto odom_frame_id = controller_namespace + params_.odom_frame_id; - const auto base_frame_id = controller_namespace + params_.base_frame_id; + const auto odom_frame_id = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = controller_namespace + odom_frame_id; - odometry_message.child_frame_id = controller_namespace + base_frame_id; + odometry_message.header.frame_id = odom_frame_id; + odometry_message.child_frame_id = base_frame_id; // limit the publication on the topics /odom and /tf - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_rate_ = params_.publish_rate; publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - previous_publish_timestamp_ = get_node()->get_clock()->now(); // initialize odom values zeros odometry_message.twist = @@ -466,6 +491,13 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; + if (!is_halted) + { + halt(); + is_halted = true; + } + registered_left_wheel_handles_.clear(); + registered_right_wheel_handles_.clear(); return controller_interface::CallbackReturn::SUCCESS; } @@ -510,12 +542,6 @@ bool DiffDriveController::reset() return true; } -controller_interface::CallbackReturn DiffDriveController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 82ba210113..3f370bcc65 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,27 +2,39 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } wheel_separation: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheels_per_side: { type: int, default_value: 0, - description: "Number of wheels on each wide of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number or control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", + description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } wheel_separation_multiplier: { type: double, @@ -39,6 +51,16 @@ diff_drive_controller: default_value: 1.0, description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", } + tf_frame_prefix_enable: { + type: bool, + default_value: true, + description: "Enables or disables appending tf_prefix to tf frame id's.", + } + tf_frame_prefix: { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } odom_frame_id: { type: string, default_value: "odom", @@ -46,7 +68,7 @@ diff_drive_controller: } base_frame_id: { type: string, - default_value: "odom", + default_value: "base_link", description: "Name of the robot's base frame that is child of the odometry frame.", } pose_covariance_diagonal: { @@ -62,7 +84,7 @@ diff_drive_controller: open_loop: { type: bool, default_value: false, - description: "If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.", + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", } position_feedback: { type: bool, @@ -77,7 +99,7 @@ diff_drive_controller: cmd_vel_timeout: { type: double, default_value: 0.5, # seconds - description: "Timeout after which input command on ``cmd_vel`` topic is considered staled.", + description: "Timeout in seconds, after which input command on ``cmd_vel`` topic is considered staled.", } publish_limited_velocity: { type: bool, diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 99bb22890d..95fa26b72b 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -74,7 +74,10 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); - + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } // Compute linear and angular diff: const double linear = (left_vel + right_vel) * 0.5; // Now there is a bug about scout angular velocity diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..d3f181d75e 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -48,25 +54,31 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr } /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout - */ - bool wait_for_twist( + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; + } + + /** + * @brief Used to get the real_time_odometry_publisher to verify its contents + * + * @return Copy of realtime_odometry_publisher_ object + */ + std::shared_ptr> + get_rt_odom_publisher() + { + return realtime_odometry_publisher_; } }; @@ -155,11 +167,32 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + // default parameters + parameter_overrides.push_back( + rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, ns, node_options); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -180,79 +213,210 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Publisher::SharedPtr velocity_publisher; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); +} + +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) +{ + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); + + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); +} + +TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) +{ + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } -TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_THAT( - controller_->state_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); - ASSERT_THAT( - controller_->command_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + std::string test_namespace = "/test_namespace"; + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's instead of the namespace*/ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + std::string ns_prefix = test_namespace.erase(0, 1) + "/"; + /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the + * frame id's */ + ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id); +} - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); +TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -260,15 +424,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -276,15 +434,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -293,15 +447,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -310,15 +460,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -327,15 +473,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -376,15 +518,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -404,7 +542,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 20caf46b6f..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -16,13 +16,14 @@ #include #include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -30,8 +31,17 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_unique(ros2_control_test_assets::diffbot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController")); + ASSERT_NE( + cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), + nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; } diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index dee9d31abe..878d2fd494 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -1,58 +1,75 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/controllers_index.rst + .. _controllers: ################# ros2_controllers ################# -`GitHub Repository `_ - +Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. -Nomenclature -************ +`Link to GitHub Repository `_ -The ros2_control framework uses namespaces to sort controller according to the type of command interface they are using. -The controllers are using `common hardware interface definitions`_. -The controllers' namespaces are commanding the following command interface types: - - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` - - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` - - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` - - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - - ... +Guidelines and Best Practices +***************************** -.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +.. toctree:: + :titlesonly: + mobile_robot_kinematics.rst + writing_new_controller.rst -Guidelines and Best Practices -***************************** +Controllers for Wheeled Mobile Robots +************************************* .. toctree:: :titlesonly: - :glob: - * + Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + +Controllers for Manipulators and Other Robots +********************************************* +The controllers are using `common hardware interface definitions`_, and may use namespaces depending on the following command interface types: + + - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` + - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` + - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` + - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` + +.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp -Available Controllers -********************* .. toctree:: :titlesonly: - Differential Drive <../diff_drive_controller/doc/userdoc.rst> - Forward Command <../forward_command_controller/doc/userdoc.rst> - Joint Trajectory <../joint_trajectory_controller/doc/userdoc.rst> + Admittance Controller <../admittance_controller/doc/userdoc.rst> + Effort Controllers <../effort_controllers/doc/userdoc.rst> + Forward Command Controller <../forward_command_controller/doc/userdoc.rst> + Gripper Controller <../gripper_controllers/doc/userdoc.rst> + Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> - Effort Controllers <../effort_controllers/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> -Available Broadcasters +Broadcasters ********************** +Broadcasters are used to publish sensor data from hardware components to ROS topics. +In the sense of ros2_control, broadcasters are still controllers using the same controller interface as the other controllers above. + .. toctree:: :titlesonly: + Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> + IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> - Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> diff --git a/doc/images/.gitignore b/doc/images/.gitignore new file mode 100644 index 0000000000..8d71bf9cf4 --- /dev/null +++ b/doc/images/.gitignore @@ -0,0 +1 @@ +*.bkp diff --git a/doc/images/ackermann_steering.drawio b/doc/images/ackermann_steering.drawio new file mode 100644 index 0000000000..d0ddb06a56 --- /dev/null +++ b/doc/images/ackermann_steering.drawio @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering.svg b/doc/images/ackermann_steering.svg new file mode 100644 index 0000000000..1ca6ace166 --- /dev/null +++ b/doc/images/ackermann_steering.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/ackermann_steering_traction.drawio b/doc/images/ackermann_steering_traction.drawio new file mode 100644 index 0000000000..ccf40be4d7 --- /dev/null +++ b/doc/images/ackermann_steering_traction.drawio @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering_traction.svg b/doc/images/ackermann_steering_traction.svg new file mode 100644 index 0000000000..1ee9d0181b --- /dev/null +++ b/doc/images/ackermann_steering_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/car_like_robot.drawio b/doc/images/car_like_robot.drawio new file mode 100644 index 0000000000..f9b87b9794 --- /dev/null +++ b/doc/images/car_like_robot.drawio @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/car_like_robot.svg b/doc/images/car_like_robot.svg new file mode 100644 index 0000000000..6fb6bf17d0 --- /dev/null +++ b/doc/images/car_like_robot.svg @@ -0,0 +1,3 @@ + + +
ICR
Front wheel
Rear wheel
 diff --git a/doc/images/diff_drive.drawio b/doc/images/diff_drive.drawio new file mode 100644 index 0000000000..f5fc9802e0 --- /dev/null +++ b/doc/images/diff_drive.drawio @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/diff_drive.svg b/doc/images/diff_drive.svg new file mode 100644 index 0000000000..9759b86048 --- /dev/null +++ b/doc/images/diff_drive.svg @@ -0,0 +1,3 @@ + + +
 diff --git a/doc/images/double_traction.drawio b/doc/images/double_traction.drawio new file mode 100644 index 0000000000..7f9989d733 --- /dev/null +++ b/doc/images/double_traction.drawio @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/double_traction.svg b/doc/images/double_traction.svg new file mode 100644 index 0000000000..ec60b11ffa --- /dev/null +++ b/doc/images/double_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/omni_wheel_omnidirectional_drive.drawio b/doc/images/omni_wheel_omnidirectional_drive.drawio new file mode 100644 index 0000000000..71a628ba0f --- /dev/null +++ b/doc/images/omni_wheel_omnidirectional_drive.drawio @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/omni_wheel_omnidirectional_drive.svg b/doc/images/omni_wheel_omnidirectional_drive.svg new file mode 100644 index 0000000000..215bb702b8 --- /dev/null +++ b/doc/images/omni_wheel_omnidirectional_drive.svg @@ -0,0 +1,4 @@ + + + +
 diff --git a/doc/images/unicycle.drawio b/doc/images/unicycle.drawio new file mode 100644 index 0000000000..49a1f44b5a --- /dev/null +++ b/doc/images/unicycle.drawio @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/unicycle.svg b/doc/images/unicycle.svg new file mode 100644 index 0000000000..826bbb1463 --- /dev/null +++ b/doc/images/unicycle.svg @@ -0,0 +1,3 @@ + + +
 diff --git a/doc/migration.rst b/doc/migration.rst new file mode 100644 index 0000000000..2c35f57aee --- /dev/null +++ b/doc/migration.rst @@ -0,0 +1,13 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst + +Migration Guides: Galactic to Humble +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes important changes between Galactic (previous) and Humble (current) releases, where changes to user code might be necessary. + +.. note:: + + This list was created in July 2024, earlier changes are not included. + +joint_trajectory_controller +***************************** + * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst new file mode 100644 index 0000000000..b861733ee7 --- /dev/null +++ b/doc/mobile_robot_kinematics.rst @@ -0,0 +1,406 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/mobile_robot_kinematics.rst + +.. _mobile_robot_kinematics: + +Wheeled Mobile Robot Kinematics +-------------------------------------------------------------- + +.. _siciliano: https://link.springer.com/book/10.1007/978-1-84628-642-1 +.. _modern_robotics: http://modernrobotics.org/ + +This page introduces the kinematics of different wheeled mobile robots. For further reference see `Siciliano et.al - Robotics: Modelling, Planning and Control `_ and `Kevin M. Lynch and Frank C. Park - Modern Robotics: Mechanics, Planning, And Control `_. + +Wheeled mobile robots can be classified in two categories: + +Omnidirectional robots + which can move instantaneously in any direction in the plane, and + +Nonholonomic robots + which cannot move instantaneously in any direction in the plane. + +The forward integration of the kinematic model using the encoders of the wheel actuators — is referred to as **odometric localization** or **passive localization** or **dead reckoning**. We will call it just **odometry**. + +Omnidirectional Wheeled Mobile Robots +..................................... + +Omnidirectional Drive Robots using Omni Wheels +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The below explains the kinematics of omnidirectional drive robots using 3 or more omni wheels. +It follows the coordinate conventions defined in `ROS REP 103 `__. + +.. image:: images/omni_wheel_omnidirectional_drive.svg + :width: 550 + :align: center + :alt: Omnidirectional Drive Robot using Omni Wheels + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`v_{b,x},` is the robot's linear velocity on the x-axis. +* :math:`v_{b,y}` is the robot's linear velocity on the y-axis. +* :math:`\omega_{b,z}` is the robot's angular velocity on the z-axis. +* :math:`R` is the robot's radius / the distance between the robot's center and the wheels. +* Red arrows on the wheel :math:`i` signify the positive direction of their rotation :math:`\omega_i` +* :math:`\gamma` is the angular offset of the first wheel from :math:`x_b`. +* :math:`\theta` is the angle between each wheel which can be calculated using the below equation where :math:`n` is the number of wheels. + +.. math:: + + θ = \frac{2\pi}{n} + +**Inverse Kinematics** + +The necessary angular velocity of the wheels to achieve a desired body twist can be calculated using the below matrix: + +.. math:: + + A = + \begin{bmatrix} + \sin(\gamma) & -\cos(\gamma) & -R \\ + \sin(\theta + \gamma) & -\cos(\theta + \gamma) & -R\\ + \sin(2\theta + \gamma) & -\cos(2\theta + \gamma) & -R\\ + \sin(3\theta + \gamma) & -\cos(3\theta + \gamma) & -R\\ + \vdots & \vdots & \vdots\\ + \sin((n-1)\theta + \gamma) & -\cos((n-1)\theta + \gamma) & -R\\ + \end{bmatrix} + +.. math:: + + \begin{bmatrix} + \omega_1\\ + \omega_2\\ + \omega_3\\ + \omega_4\\ + \vdots\\ + \omega_n + \end{bmatrix} = + \frac{1}{r} + A + \begin{bmatrix} + v_{b,x}\\ + v_{b,y}\\ + \omega_{b,z}\\ + \end{bmatrix} + +Here :math:`\omega_1,\ldots,\omega_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels. +These equations can be written in algebraic form for any wheel :math:`i` like this: + +.. math:: + \omega_i = \frac{\sin((i-1)\theta + \gamma) v_{b,x} - \cos((i-1)\theta + \gamma) v_{b,y} - R \omega_{b,z}}{r} + +**Forward Kinematics** + +The body twist of the robot can be obtained from the wheel velocities by using the pseudoinverse of matrix :math:`A`. + +.. math:: + + \begin{bmatrix} + v_{b,x}\\ + v_{b,y}\\ + \omega_{b,z}\\ + \end{bmatrix} = + rA^\dagger + \begin{bmatrix} + \omega_1\\ + \omega_2\\ + \omega_3\\ + \omega_4\\ + \vdots\\ + \omega_n + \end{bmatrix} + +Nonholonomic Wheeled Mobile Robots +..................................... + +Unicycle model +,,,,,,,,,,,,,,,, + +To define the coordinate systems (`ROS coordinate frame conventions `__, the coordinate systems follow the right-hand rule), consider the following simple unicycle model + +.. image:: images/unicycle.svg + :width: 550 + :align: center + :alt: Unicycle + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`x,y` are the robot's Cartesian coordinates in the world coordinate system. +* :math:`\theta` is the robot's heading angle, i.e. the orientation of the robot's :math:`x_b`-axis w.r.t. the world's :math:`x_w`-axis. + +In the following, we want to command the robot with a desired body twist + +.. math:: + + \vec{\nu}_b = \begin{bmatrix} + \vec{\omega}_{b} \\ + \vec{v}_{b} + \end{bmatrix}, + +where :math:`\vec{v}_{b}` is the linear velocity of the robot in its body-frame, and :math:`\vec\omega_{b}` is the angular velocity of the robot in its body-frame. As we consider steering robots on a flat surface, it is sufficient to give + +* :math:`v_{b,x}`, i.e. the linear velocity of the robot in direction of the :math:`x_b` axis. +* :math:`\omega_{b,z}`, i.e. the angular velocity of the robot about the :math:`x_z` axis. + +as desired system inputs. The forward kinematics of the unicycle can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \omega_{b,z} + +We will formulate the inverse kinematics to calculate the desired commands for the robot (wheel speed or steering) from the given body twist. + +Differential Drive Robot +,,,,,,,,,,,,,,,,,,,,,,,, + +Citing `Siciliano et.al - Robotics: Modelling, Planning and Control `_: + +.. code-block:: text + + A unicycle in the strict sense (i.e., a vehicle equipped with a single wheel) + is a robot with a serious problem of balance in static conditions. However, + there exist vehicles that are kinematically equivalent to a unicycle but more + stable from a mechanical viewpoint. + +One of these vehicles is the differential drive robot, which has two wheels, each of which is driven independently. + +.. image:: images/diff_drive.svg + :width: 550 + :align: center + :alt: Differential drive robot + +* :math:`w` is the wheel track (the distance between the wheels). + +**Forward Kinematics** + +The forward kinematics of the differential drive model can be calculated from the unicycle model above using + +.. math:: + v_{b,x} &= \frac{v_{right} + v_{left}}{2} \\ + \omega_{b,z} &= \frac{v_{right} - v_{left}}{w} + +**Inverse Kinematics** + +The necessary wheel speeds to achieve a desired body twist can be calculated with: + +.. math:: + + v_{left} &= v_{b,x} - \omega_{b,z} w / 2 \\ + v_{right} &= v_{b,x} + \omega_{b,z} w / 2 + + +**Odometry** + +We can use the forward kinematics equations above to calculate the robot's odometry directly from the encoder readings. + +Car-Like (Bicycle) Model +,,,,,,,,,,,,,,,,,,,,,,,, + +The following picture shows a car-like robot with two wheels, where the front wheel is steerable. This model is also known as the bicycle model. + +.. image:: images/car_like_robot.svg + :width: 550 + :align: center + :alt: Car-like robot + +* :math:`\phi` is the steering angle of the front wheel, counted positive in direction of rotation around :math:`x_z`-axis. +* :math:`v_{rear}, v_{front}` is the velocity of the rear and front wheel. +* :math:`l` is the wheelbase. + +We assume that the wheels are rolling without slipping. This means that the velocity of the contact point of the wheel with the ground is zero and the wheel's velocity points in the direction perpendicular to the wheel's axis. The **Instantaneous Center of Rotation** (ICR), i.e. the center of the circle around which the robot rotates, is located at the intersection of the lines that are perpendicular to the wheels' axes and pass through the contact points of the wheels with the ground. + +As a consequence of the no-slip condition, the velocity of the two wheels must satisfy the following constraint: + +.. math:: + v_{rear} = v_{front} \cos(\phi) + +**Forward Kinematics** + +The forward kinematics of the car-like model can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{b,x}}{l} \tan(\phi) + + +**Inverse Kinematics** + +The steering angle is one command input of the robot: + +.. math:: + \phi = \arctan\left(\frac{l w_{b,z}}{v_{b,x}} \right) + + +For the rear-wheel drive, the velocity of the rear wheel is the second input of the robot: + +.. math:: + v_{rear} = v_{b,x} + + +For the front-wheel drive, the velocity of the front wheel is the second input of the robot: + +.. math:: + v_{front} = \frac{v_{b,x}}{\cos(\phi)} + +**Odometry** + +We have to distinguish between two cases: Encoders on the rear wheel or on the front wheel. + +For the rear wheel case: + +.. math:: + \dot{x} &= v_{rear} \cos(\theta) \\ + \dot{y} &= v_{rear} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{rear}}{l} \tan(\phi) + + +For the front wheel case: + +.. math:: + \dot{x} &= v_{front} \cos(\theta) \cos(\phi)\\ + \dot{y} &= v_{front} \sin(\theta) \cos(\phi)\\ + \dot{\theta} &= \frac{v_{front}}{l} \sin(\phi) + + +Double-Traction Axle +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a car-like robot with three wheels, with two independent traction wheels at the rear. + +.. image:: images/double_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two traction wheels at the rear + +* :math:`w_r` is the wheel track of the rear axle. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the velocity of the rear wheels must satisfy these conditions to avoid skidding + +.. math:: + v_{rear,left} &= v_{b,x}\frac{R_b - w_r/2}{R_b}\\ + v_{rear,right} &= v_{b,x}\frac{R_b + w_r/2}{R_b} + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{rear,left} \frac{R_b}{R_b - w_r/2} = v_{rear,right} \frac{R_b}{R_b + w_r/2} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). + + +Ackermann Steering +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled robot with two independent steering wheels in the front. + +.. image:: images/ackermann_steering.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front + +* :math:`w_f` is the wheel track of the front axle, measured between the two kingpins. + +To prevent the front wheels from slipping, the steering angle of the front wheels cannot be equal. +This is the so-called **Ackermann steering**. + +.. note:: + Ackermann steering can also be achieved by a `mechanical linkage between the two front wheels `__. In this case the robot has only one steering input, and the steering angle of the two front wheels is mechanically coupled. The inverse kinematics of the robot will then be the same as in the car-like model above. + +**Forward Kinematics** + +The forward kinematics is the same as for the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the steering angles of the front wheels must satisfy these conditions to avoid skidding + +.. math:: + \phi_{left} &= \arctan\left(\frac{l}{R_b - w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) - w_f\sin(\phi)}\right)\\ + \phi_{right} &= \arctan\left(\frac{l}{R_b + w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) + w_f\sin(\phi)}\right) + +**Odometry** + +The calculation of :math:`\phi` from two angle measurements of the steering axle is overdetermined. +If there is no slip and the measurements are ideal, + +.. math:: + \phi = \arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) = \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right) + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + \phi = 0.5 \left(\arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) + \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right)\right). + +Ackermann Steering with Traction +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled car-like robot with two independent steering wheels at the front, which are also driven independently. + +.. image:: images/ackermann_steering_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front, which are also independently driven. + +* :math:`d_{kp}` is the distance from the kingpin to the contact point of the front wheel with the ground. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +To avoid slipping of the front wheels, the velocity of the front wheels cannot be equal and + +.. math:: + \frac{v_{front,left}}{R_{left}} = \frac{v_{front,right}}{R_{right}} = \frac{v_{b,x}}{R_b} + +with turning radius of the robot and the left/right front wheel + +.. math:: + R_b &= \frac{l}{\tan(\phi)} \\ + R_{left} &= \frac{l-d_{kp}\sin(\phi_{left})}{\sin(\phi_{left})}\\ + R_{right} &= \frac{l+d_{kp}\sin(\phi_{right})}{\sin(\phi_{right})}. + +This results in the following inverse kinematics equations + +.. math:: + v_{front,left} &= \frac{v_{b,x}(l-d_{kp}\sin(\phi_{left}))}{R_b\sin(\phi_{left})}\\ + v_{front,right} &= \frac{v_{b,x}(l+d_{kp}\sin(\phi_{right}))}{R_b\sin(\phi_{right})} + +with the steering angles of the front wheels from the Ackermann steering equations above. + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is again overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} = v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). diff --git a/doc/release_notes.rst b/doc/release_notes.rst new file mode 100644 index 0000000000..3c2c91fdf4 --- /dev/null +++ b/doc/release_notes.rst @@ -0,0 +1,65 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst + +Release Notes: Galactic to Humble +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes the changes between Galactic (previous) and Humble (current) releases. Bugfixes are not included in this list. + +.. note:: + + This list was created in July 2024, earlier changes may not be included. + +diff_drive_controller +***************************** +* Remove unused parameter ``wheels_per_side`` (`#958 `_). + +joint_trajectory_controller +***************************** + +* Activate update of dynamic parameters (`#761 `_ and `#849 `_). +* Continue with last trajectory-point on success, instead of hold-position from current state (`#842 `_). +* Add console output for tolerance checks (`#932 `_): + + .. code:: + + [tolerances]: State tolerances failed for joint 2: + [tolerances]: Position Error: 0.020046, Position Tolerance: 0.010000 + [trajectory_controllers]: Aborted due goal_time_tolerance exceeding by 1.010000 seconds + +* Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). +* Empty trajectory messages are discarded (`#902 `_). +* Action field ``error_string`` is now filled with meaningful strings (`#887 `_). +* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). +* Tolerances sent with the action goal are now processed and used for the action. (`#716 `_). For details, see the `JointTolerance message `_: + + .. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + +* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). + +mecanum_drive_controller +************************ +* 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). + +pid_controller +************************ +* 🚀 The PID controller was added 🎉 (`#434 `_). + +steering_controllers_library +******************************** +* Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). +* New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 `_). +* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). + +gpio_controllers +************************ +* The GPIO command controller was added 🎉 (`#1251 `_). diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index dcf577e131..23fc220568 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -1,9 +1,11 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/writing_new_controller.rst + .. _writing_new_controllers: Writing a new controller ======================== -In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. +In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller. 1. **Preparing package** @@ -25,17 +27,19 @@ The following is a step-by-step guide to create source files, basic tests, and c 3. **Adding declarations into header file (.hpp)** - 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). + 1. Take care that you use header guards. ROS 2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). 2. include ``"controller_interface/controller_interface.hpp"`` and ``visibility_control.h`` if you are using one. 3. Define a unique namespace for your controller. This is usually a package name written in ``snake_case``. 4. Define the class of the controller, extending ``ControllerInterface``, e.g., + .. code:: c++ - class ControllerName : public controller_interface::ControllerInterface - 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. + class ControllerName : public controller_interface::ControllerInterface + + 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. @@ -46,9 +50,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Include the header file of your controller and add a namespace definition to simplify further development. 2. (optional) Implement a constructor if needed. There, you could initialize member variables. - This could also be done in the ``init`` method. + This could also be done in the ``on_init`` method. - 3. Implement the ``init`` method. The first line usually calls the parent ``init`` method. + 3. Implement the ``on_init`` method. The first line usually calls the parent ``on_init`` method. Here is the best place to initialize the variables, reserve memory, and most importantly, declare node parameters used by the controller. If everything works fine return ``controller_interface::return_type::OK`` or ``controller_interface::return_type::ERROR`` otherwise. 4. Write the ``on_configure`` method. Parameters are usually read here, and everything is prepared so that the controller can be started. @@ -103,6 +107,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 4. Add ament dependencies needed by the library. You should add at least those listed under 1. 5. Export for pluginlib description file using the following command: + .. code:: cmake pluginlib_export_plugin_description_file(controller_interface .xml) @@ -134,6 +139,11 @@ That's it! Enjoy writing great controllers! Useful External References ---------------------------- +-------------------------- + +- `Templates and scripts for generating controllers shell `_ + + + .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. -- `Templates and scripts for generating controllers shell `_ +- :ref:`Writing a new model-based controller `: when writing model-based controllers, e.g. gravity compensation or inverse dynamics controllers, the manipulator's dynamic matrices can be retrieved using the `inverse_dynamics_solver plugin `_. diff --git a/downstream.humble.repos b/downstream.humble.repos new file mode 100644 index 0000000000..81feb3723b --- /dev/null +++ b/downstream.humble.repos @@ -0,0 +1,5 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: humble diff --git a/downstream.jazzy.repos b/downstream.jazzy.repos new file mode 100644 index 0000000000..e5a26574b7 --- /dev/null +++ b/downstream.jazzy.repos @@ -0,0 +1,5 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main diff --git a/downstream.rolling.repos b/downstream.rolling.repos new file mode 100644 index 0000000000..e5a26574b7 --- /dev/null +++ b/downstream.rolling.repos @@ -0,0 +1,5 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6954b1f7e3..fe75121695 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,138 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- +* [ForwardCommandController] Fix the duplicate command interface types when reconfiguring the controller (backport `#1568 `_, `#1570 `_) (`#1569 `_) +* Contributors: mergify[bot] + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 91632606bf..ad97690655 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -1,85 +1,73 @@ -cmake_minimum_required(VERSION 3.5) -project(effort_controllers) +cmake_minimum_required(VERSION 3.16) +project(effort_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(effort_controllers SHARED src/joint_group_effort_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(effort_controllers PUBLIC cxx_std_17) +target_include_directories(effort_controllers PUBLIC + $ + $ ) +ament_target_dependencies(effort_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_effort_controller + ament_add_gmock(test_load_joint_group_effort_controller test/test_load_joint_group_effort_controller.cpp ) - target_include_directories(test_load_joint_group_effort_controller PRIVATE include) + target_link_libraries(test_load_joint_group_effort_controller + effort_controllers + ) ament_target_dependencies(test_load_joint_group_effort_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_effort_controller + ament_add_gmock(test_joint_group_effort_controller test/test_joint_group_effort_controller.cpp ) - target_include_directories(test_joint_group_effort_controller PRIVATE include) target_link_libraries(test_joint_group_effort_controller - ${PROJECT_NAME} + effort_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/effort_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS effort_controllers + EXPORT export_effort_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_effort_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 000bfcf26c..06bfe3f38c 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/effort_controllers/doc/userdoc.rst + .. _effort_controllers_userdoc: effort_controllers @@ -5,7 +7,90 @@ effort_controllers This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. -Hardware interface type ------------------------ +The package contains the following controllers: + +effort_controllers/JointGroupEffortController +--------------------------------------------- + +This is specialization of the :ref:`forward_command_controller ` that works using the "effort" joint interface. + + +ROS 2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Topics +,,,,,, + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Joints' effort commands + + +Parameters +,,,,,,,,,, +This controller overrides the interface parameter from :ref:`forward_command_controller `, and the +``joints`` parameter is the only one that is required. + +An example parameter file is given here + +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + effort_controller: + type: effort_controllers/JointGroupEffortController + + effort_controller: + ros__parameters: + joints: + - slider_to_cart + +Model-based controllers +-------------------------- + +Model-based controllers' control laws often require computing the manipulator's dynamics terms, e.g. inertia, coriolis, friction, and gravity contributions. +Notable examples are `gravity-compensation PD controllers or inverse dynamics controllers `_. + +Control laws +^^^^^^^^^^^^ + +For instance, the PD Control with Gravity Compensation law is the following + +.. math:: + + \boldsymbol\tau = \boldsymbol K_p \tilde{\boldsymbol q} - \boldsymbol K_d \dot{\boldsymbol q} + \boldsymbol g(\boldsymbol q) + +and can be implemented in C++ as + +.. code-block:: cpp + + torque_output = K_p * position_error - K_d * velocity + g; + +Similarly, the inverse dynamics control law is the following + +.. math:: + + \boldsymbol\tau = \boldsymbol B(\boldsymbol q) \left( \boldsymbol K_p \tilde{\boldsymbol q} + \boldsymbol K_d \dot{\tilde{\boldsymbol q}} + \ddot{\boldsymbol q}_d \right) + \boldsymbol C(\boldsymbol q, \dot{\boldsymbol q}) \dot{\boldsymbol q} + \boldsymbol f(\dot{\boldsymbol q}) + \boldsymbol g(\boldsymbol q) + +and can be implemented in C++ as + +.. code-block:: cpp + + torque_output = B * (K_p * position_error + K_d * velocity_error + desired_acceleration) + c + f + g; + +The role of the inverse dynamics solver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +To facilitate the implementation of these controllers, the robot-agnostic `inverse dynamics solver `_ plugin can retrieve the dynamics terms with + +.. code-block:: cpp + + Eigen::MatrixXd B = inverse_dynamics_solver_->getInertiaMatrix(position); + Eigen::VectorXd c = inverse_dynamics_solver_->getCoriolisVector(position, velocity); + Eigen::VectorXd f = inverse_dynamics_solver_->getFrictionVector(velocity); + Eigen::VectorXd g = inverse_dynamics_solver_->getGravityVector(position); + +where ``position``, ``velocity``, ``desired_acceleration``, ``position_error`` and ``velocity_error`` are given by the current robot state and reference, ``K_p`` and ``K_d`` are control gains, and ``torque_output`` shall be written on the command interfaces. -These controllers work with joints using the "effort" command interface. +For more information about the solver, please have a look at `this example `_ with KDL for simulated robots. diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 7476889b73..948da843e1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,22 +1,33 @@ effort_controllers - 2.15.0 + 2.44.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake + backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 256a4ce465..1d90b280ae 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -62,6 +51,7 @@ void JointGroupEffortControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) @@ -99,12 +89,13 @@ TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails) // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); - // activate failed, 'acceleration' is not a registered interface for `joint1` + // activate should succeed now ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) @@ -203,10 +194,13 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..e871ac6c43 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -24,6 +24,7 @@ #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -57,6 +58,7 @@ class JointGroupEffortControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index a216d46db3..52f1f9934a 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) @@ -32,8 +34,10 @@ TEST(TestLoadJointGroupVelocityController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController")); + ASSERT_NE( + cm.load_controller( + "test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController"), + nullptr); rclcpp::shutdown(); } diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 3a37a17174..c866622187 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,159 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_)" (`#988 `_) (`#1003 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- +* [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (backport `#698 `_) (`#750 `_) + * [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_) + * Create ParamListener and get parameters on configure + * Declare parameters for test_force_torque_sensor_broadcaster + Since the parameters are not declared on init anymore, they cannot be + set without declaring them before + --------- + Co-authored-by: Bence Magyar + (cherry picked from commit 32aaef7552638826aba0b3f3a72b1c1453739afa) + * Fix "parameter is already declared" error + --------- + Co-authored-by: Noel Jiménez García + Co-authored-by: Christoph Froehlich +* Contributors: mergify[bot] + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Broadcaster parameters (`#650 `_) (`#678 `_) +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Christoph Fröhlich, Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 3aa57d4aad..2af5500e21 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -1,28 +1,23 @@ -cmake_minimum_required(VERSION 3.5) -project(force_torque_sensor_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(force_torque_sensor_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - geometry_msgs - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -31,81 +26,71 @@ generate_parameter_library(force_torque_sensor_broadcaster_parameters src/force_torque_sensor_broadcaster_parameters.yaml ) -add_library( - ${PROJECT_NAME} - SHARED +add_library(force_torque_sensor_broadcaster SHARED src/force_torque_sensor_broadcaster.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) -ament_target_dependencies( - ${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(force_torque_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(force_torque_sensor_broadcaster PUBLIC + $ + $ ) -target_link_libraries(force_torque_sensor_broadcaster +target_link_libraries(force_torque_sensor_broadcaster PUBLIC force_torque_sensor_broadcaster_parameters ) +ament_target_dependencies(force_torque_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(force_torque_sensor_broadcaster PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) -install( - TARGETS force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters - EXPORT export_force_torque_sensor_broadcaster - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_force_torque_sensor_broadcaster + add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_force_torque_sensor_broadcaster + force_torque_sensor_broadcaster + ) ament_target_dependencies(test_load_force_torque_sensor_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_force_torque_sensor_broadcaster + add_rostest_with_parameters_gmock(test_force_torque_sensor_broadcaster test/test_force_torque_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster - ${PROJECT_NAME} + force_torque_sensor_broadcaster ) ament_target_dependencies(test_force_torque_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_force_torque_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets - ) endif() -ament_export_targets( - export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET +install( + DIRECTORY include/ + DESTINATION include/force_torque_sensor_broadcaster ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + TARGETS + force_torque_sensor_broadcaster + force_torque_sensor_broadcaster_parameters + EXPORT export_force_torque_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/force_torque_sensor_broadcaster/doc/parameters_context.yaml b/force_torque_sensor_broadcaster/doc/parameters_context.yaml new file mode 100644 index 0000000000..6991427316 --- /dev/null +++ b/force_torque_sensor_broadcaster/doc/parameters_context.yaml @@ -0,0 +1,12 @@ +interface_names: | + (optional) Defines custom, per axis interface names. + This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. + It is sufficient that only one ``interface_name`` is defined. + This enables the broadcaster to use force sensing cells with less than six measuring axes. + An example definition is: + + .. code-block:: yaml + + interface_names: + force: + x: example_name/example_interface diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index de8a1bd7c6..df0430e3bb 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/force_torque_sensor_broadcaster/doc/userdoc.rst + .. _force_torque_sensor_broadcaster_userdoc: Force Torque Sensor Broadcaster @@ -10,25 +12,17 @@ The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see Parameters ^^^^^^^^^^^ -The interfaces can be defined in two ways, using ``sensor_name`` or ``interface_names`` parameter. -Those two parameters can not be defined at the same time +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -frame_id (mandatory) - Frame in which the output message will be published. +The interfaces can be defined in two ways, using the ``sensor_name`` or the ``interface_names`` parameter: +Those two parameters cannot be defined at the same time. -sensor_name (optional) - Defines sensor name used as prefix for its interfaces. - If used standard interface names for a 6D FTS will be used: /force.x, ..., /torque.z. +Full list of parameters: -interface_names.[force|torque].[x|y|z] (optional) - Defines custom, per axis interface names. - This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. - It is sufficient that only one ``interface_name`` is defined. - This enables broadcaster use for force sensing cells with less then six measuring axes. - Example definition is: +.. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.yaml + parameters_context.yaml - .. code-block:: yaml +An example parameter file for this controller can be found in `the test directory `_: - interface_names: - force: - x: example_name/example_interface +.. literalinclude:: ../test/force_torque_sensor_broadcaster_params.yaml + :language: yaml diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 6166adfd9a..1d2e5ea7dd 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -24,12 +24,12 @@ #include #include "controller_interface/controller_interface.hpp" +#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" -#include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 8cee3ef6a3..4b1e7461f9 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,15 +2,26 @@ force_torque_sensor_broadcaster - 2.15.0 + 2.44.0 Controller to publish state of force-torque sensors. + Bence Magyar - Denis Štogl - Subhas Das + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Subhas Das + ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface @@ -22,7 +33,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9687f9dfee..9b570d353f 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -72,12 +72,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.frame_id.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); - return controller_interface::CallbackReturn::ERROR; - } - if (!params_.sensor_name.empty()) { force_torque_sensor_ = std::make_unique( diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 059d5ab9a1..3e75ab6012 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -3,11 +3,15 @@ force_torque_sensor_broadcaster: type: string, default_value: "", description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } } sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined. + If used, standard interface names for a 6D FTS will be used: ``/force.x, ..., /torque.z``", } interface_names: force: diff --git a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..58cc1c6b16 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_force_torque_sensor_broadcaster: + ros__parameters: + + frame_id: "fts_sensor_frame" diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 74e2b15da0..5b8ee55648 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -39,9 +41,9 @@ constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void ForceTorqueSensorBroadcasterTest::SetUpTestCase() {} -void ForceTorqueSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void ForceTorqueSensorBroadcasterTest::TearDownTestCase() {} void ForceTorqueSensorBroadcasterTest::SetUp() { @@ -71,31 +73,40 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg) { // create a new subscriber + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); + wrench_msg = *received_msg; } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) @@ -106,29 +117,6 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet) -{ - SetUpFTSBroadcaster(); - - // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) -{ - SetUpFTSBroadcaster(); - - // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) { SetUpFTSBroadcaster(); @@ -179,6 +167,14 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -196,7 +192,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) { SetUpFTSBroadcaster(); @@ -207,6 +203,25 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // deactivate passed + ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) @@ -315,3 +330,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 5477b3fa6f..fe5b0ab3ba 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -36,7 +36,7 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorNameParameterIsEmpty); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, InterfaceNameParameterIsEmpty); - FRIEND_TEST(ForceTorqueSensorBroadcasterTest, ActivateSuccess); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); }; diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 6e9b6b1167..0c269d6a31 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -28,8 +28,6 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -38,9 +36,18 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_force_torque_sensor_broadcaster", - "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster")); + ASSERT_NE( + cm.load_controller( + "test_force_torque_sensor_broadcaster", + "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), + nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 0dc1db853b..5935d55dcd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,152 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- +* [ForwardCommandController] Fix the duplicate command interface types when reconfiguring the controller (backport `#1568 `_, `#1570 `_) (`#1569 `_) +* Contributors: mergify[bot] + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- +* Rearrange controllers overview page (`#846 `_) (`#847 `_) +* Contributors: mergify[bot] + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- +* Use tabs (`#743 `_) (`#746 `_) +* Contributors: Christoph Fröhlich + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- +* Fix docs format (`#591 `_) +* Contributors: Christoph Fröhlich + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index ef8581d195..15ffe09750 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -1,28 +1,23 @@ -cmake_minimum_required(VERSION 3.5) -project(forward_command_controller) +cmake_minimum_required(VERSION 3.16) +project(forward_command_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_msgs + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -36,88 +31,87 @@ generate_parameter_library( src/multi_interface_forward_command_controller_parameters.yaml ) -add_library(forward_command_controller - SHARED +add_library(forward_command_controller SHARED src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_include_directories(forward_command_controller PRIVATE include) -ament_target_dependencies(forward_command_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(forward_command_controller +target_compile_features(forward_command_controller PUBLIC cxx_std_17) +target_include_directories(forward_command_controller PUBLIC + $ + $ +) +target_link_libraries(forward_command_controller PUBLIC forward_command_controller_parameters multi_interface_forward_command_controller_parameters ) +ament_target_dependencies(forward_command_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - forward_command_controller - forward_command_controller_parameters - multi_interface_forward_command_controller_parameters - EXPORT export_forward_command_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - INCLUDES DESTINATION include -) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_forward_command_controller + ament_add_gmock(test_load_forward_command_controller test/test_load_forward_command_controller.cpp ) - target_include_directories(test_load_forward_command_controller PRIVATE include) + target_link_libraries(test_load_forward_command_controller + forward_command_controller + ) ament_target_dependencies(test_load_forward_command_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_forward_command_controller + ament_add_gmock(test_forward_command_controller test/test_forward_command_controller.cpp ) - target_include_directories(test_forward_command_controller PRIVATE include) target_link_libraries(test_forward_command_controller forward_command_controller ) - ament_add_gmock( - test_load_multi_interface_forward_command_controller + ament_add_gmock(test_load_multi_interface_forward_command_controller test/test_load_multi_interface_forward_command_controller.cpp ) - target_include_directories(test_load_multi_interface_forward_command_controller PRIVATE include) - ament_target_dependencies( - test_load_multi_interface_forward_command_controller + target_link_libraries(test_load_multi_interface_forward_command_controller + forward_command_controller + ) + ament_target_dependencies(test_load_multi_interface_forward_command_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_multi_interface_forward_command_controller + ament_add_gmock(test_multi_interface_forward_command_controller test/test_multi_interface_forward_command_controller.cpp ) - target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) target_link_libraries(test_multi_interface_forward_command_controller forward_command_controller ) endif() +install( + DIRECTORY include/ + DESTINATION include/forward_command_controller +) +install( + TARGETS + forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters + EXPORT export_forward_command_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + ament_export_targets(export_forward_command_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index ce653fe714..cd623a5acb 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -1,11 +1,42 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/forward_command_controller/doc/userdoc.rst + .. _forward_command_controller_userdoc: forward_command_controller ========================== -This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. +This is a base class implementing a feedforward controller. Specific implementations of this base class can be found in: + +* :ref:`position_controllers_userdoc` +* :ref:`velocity_controllers_userdoc` +* :ref:`effort_controllers_userdoc` Hardware interface type ----------------------- -These controllers work with joints using the "effort" command interface. +This controller can be used for every type of command interface. + + +ROS 2 interface of the controller +--------------------------------- + +Topics +^^^^^^^ + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Target joint commands + +Parameters +^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. + + .. tabs:: + + .. group-tab:: forward_command_controller + + .. generate_parameter_library_details:: ../src/forward_command_controller_parameters.yaml + + .. group-tab:: multi_interface_forward_command_controller + + .. generate_parameter_library_details:: ../src/multi_interface_forward_command_controller_parameters.yaml diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 8a697847cd..88ede8da9f 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -21,7 +21,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" -#include "forward_command_controller_parameters.hpp" +// auto-generated by generate_parameter_library +#include "forward_command_controller/forward_command_controller_parameters.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 3e153d7e2e..cabe9ae689 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -24,7 +24,7 @@ #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller @@ -87,8 +87,8 @@ class ForwardControllersBase : public controller_interface::ControllerInterface * * It is expected that error handling of exceptions is done. * - * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are - * allowed, controller_interface::CallbackReturn::ERROR otherwise. + * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and + * their values are allowed, controller_interface::CallbackReturn::ERROR otherwise. */ virtual controller_interface::CallbackReturn read_parameters() = 0; diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index fd7c0d480e..a2e3010f82 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -20,8 +20,8 @@ #include #include "forward_command_controller/forward_controllers_base.hpp" +#include "forward_command_controller/multi_interface_forward_command_controller_parameters.hpp" #include "forward_command_controller/visibility_control.h" -#include "multi_interface_forward_command_controller_parameters.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7853d78542..c08ed35f2b 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,27 +1,37 @@ forward_command_controller - 2.15.0 + 2.44.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs - pluginlib - ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0305a37e4e..052532e059 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -53,6 +53,7 @@ controller_interface::CallbackReturn ForwardCommandController::read_parameters() return controller_interface::CallbackReturn::ERROR; } + command_interface_types_.clear(); for (const auto & joint : params_.joints) { command_interface_types_.push_back(joint + "/" + params_.interface_name); diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..242c5bb1fe 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -30,21 +30,11 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; - -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace +using testing::IsEmpty; +using testing::SizeIs; void ForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -68,6 +58,7 @@ void ForwardCommandControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) @@ -128,6 +119,14 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -173,9 +172,26 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -206,7 +222,7 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check joint commands have been modified @@ -292,10 +308,13 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( @@ -323,9 +342,25 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -344,6 +379,14 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..2284d46d61 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -24,6 +24,7 @@ #include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -69,6 +70,7 @@ class ForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index 464b57b69d..b493e52b2a 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -34,8 +34,10 @@ TEST(TestLoadForwardCommandController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_forward_command_controller", "forward_command_controller/ForwardCommandController")); + ASSERT_NE( + cm.load_controller( + "test_forward_command_controller", "forward_command_controller/ForwardCommandController"), + nullptr); rclcpp::shutdown(); } diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 2f540bd0e5..41a9b74698 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -35,7 +35,9 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_forward_command_controller", - "forward_command_controller/MultiInterfaceForwardCommandController")); + ASSERT_NE( + cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/MultiInterfaceForwardCommandController"), + nullptr); } diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 0cada04859..311a4dcdaa 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -32,21 +32,11 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; - -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace +using testing::IsEmpty; +using testing::SizeIs; void MultiInterfaceForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -70,6 +60,7 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params command_ifs.emplace_back(joint_1_vel_cmd_); command_ifs.emplace_back(joint_1_eff_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); if (set_params_and_activate) { @@ -148,6 +139,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -205,7 +203,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check command in handle was set @@ -262,10 +260,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( @@ -282,6 +283,14 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes { SetUpController(true); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + // send command auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; @@ -289,7 +298,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check command in handle was set @@ -300,6 +309,14 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..78b244847b 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -26,6 +26,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -77,6 +78,7 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst new file mode 100644 index 0000000000..4abbb40908 --- /dev/null +++ b/gpio_controllers/CHANGELOG.rst @@ -0,0 +1,213 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gpio_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Gpio command controller (backport `#1251 `_) (`#1372 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- + +2.38.0 (2024-11-09) +------------------- + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..5eedec32e6 --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.8) +project(gpio_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=format -Werror=missing-braces) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(control_msgs REQUIRED) + + +generate_parameter_library(gpio_command_controller_parameters + src/gpio_command_controller_parameters.yaml +) + +add_library(gpio_controllers + SHARED + src/gpio_command_controller.cpp +) +target_include_directories(gpio_controllers PRIVATE include) +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gpio_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_gpio_command_controller + test/test_load_gpio_command_controller.cpp + ) + + target_include_directories(test_load_gpio_command_controller PRIVATE include) + ament_target_dependencies(test_load_gpio_command_controller + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock( + test_gpio_command_controller + test/test_gpio_command_controller.cpp + ) + target_include_directories(test_gpio_command_controller PRIVATE include) + target_link_libraries(test_gpio_command_controller + gpio_controllers + ) + ament_target_dependencies(test_gpio_command_controller + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + control_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools +) +ament_export_include_directories( + include +) +ament_export_libraries( + gpio_controllers +) +ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..799a53add8 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,56 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for hardware interfaces of type GPIO (```` tag in the URDF). + +gpio_command_controller +----------------------------- +gpio_command_controller let the user expose command interfaces of given GPIO interfaces and publishes state interfaces of the configured command interfaces. + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- ``//gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces. +- ``//commands`` [``control_msgs/msg/DynamicJointState``]: A subscriber for configured command interfaces. + + +Parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: + ../src/gpio_command_controller_parameters.yaml + +The controller expects at least one GPIO interface and the corresponding command interface names or state interface. However, these Command and State interfaces are optional. The controller behaves as a broadcaster when no Command Interface is present, thereby publishing the configured GPIO state interfaces if set. + +.. note:: + + When no state or command interface is provided in the param file, the controller will fail during initialization. + +.. code-block:: yaml + + gpio_command_controller: + ros__parameters: + type: gpio_controllers/GpioCommandController + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - interfaces: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - interfaces: + - ana.1 + - ana.2 + state_interfaces: + Gpio2: + - interfaces: + - ana.1 + - ana.2 + +With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml new file mode 100644 index 0000000000..03fd3e1ee0 --- /dev/null +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + + The gpio command controller commands a group of gpios using multiple interfaces. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp new file mode 100644 index 0000000000..c37ed3f55a --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,114 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_controllers/visibility_control.h" +#include "hardware_interface/hardware_info.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" + +#include "gpio_controllers/gpio_command_controller_parameters.hpp" + +namespace gpio_controllers +{ +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +using MapOfReferencesToStateInterfaces = + std::unordered_map>; +using StateInterfaces = + std::vector>; + +class GpioCommandController : public controller_interface::ControllerInterface +{ +public: + GPIO_COMMAND_CONTROLLER_PUBLIC + GpioCommandController(); + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + void store_command_interface_types(); + void store_state_interface_types(); + void initialize_gpio_state_msg(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); + template + std::unordered_map> create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces); + void apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; + void apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, + std::size_t command_interface_index) const; + InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; + bool update_dynamic_map_parameters(); + std::vector get_gpios_from_urdf() const; + +protected: + InterfacesNames command_interface_types_; + InterfacesNames state_interface_types_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + MapOfReferencesToStateInterfaces state_interfaces_map_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + + std::shared_ptr> gpio_state_publisher_{}; + std::shared_ptr> realtime_gpio_state_publisher_{}; + + std::shared_ptr param_listener_{}; + gpio_command_controller_parameters::Params params_; +}; + +} // namespace gpio_controllers + +#endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h new file mode 100644 index 0000000000..a735a1621c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml new file mode 100644 index 0000000000..de484e7fc9 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,42 @@ + + + + gpio_controllers + 2.44.0 + Controllers to interact with gpios. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Maciej Bednarczyk + Wiktor Bajor + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + control_msgs + realtime_tools + generate_parameter_library + + pluginlib + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp new file mode 100644 index 0000000000..36bce27bf4 --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,360 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "gpio_controllers/gpio_command_controller.hpp" + +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/component_parser.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" + +namespace +{ +template +void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); + } +} +} // namespace + +namespace gpio_controllers +{ + +GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} + +CallbackReturn GpioCommandController::on_init() +try +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +try +{ + if (!update_dynamic_map_parameters()) + { + return controller_interface::CallbackReturn::ERROR; + } + + store_command_interface_types(); + store_state_interface_types(); + + if (command_interface_types_.empty() && state_interface_types_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No command or state interfaces are configured"); + return CallbackReturn::ERROR; + } + + if (!command_interface_types_.empty()) + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + } + + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_interface_types_; + + return state_interfaces_config; +} + +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + command_interfaces_map_ = + create_map_of_references_to_interfaces(command_interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(state_interface_types_, state_interfaces_); + if ( + !check_if_configured_interfaces_matches_received( + command_interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(state_interface_types_, state_interfaces_map_)) + { + return CallbackReturn::ERROR; + } + + initialize_gpio_state_msg(); + rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) +{ + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + update_gpios_states(); + return update_gpios_commands(); +} + +bool GpioCommandController::update_dynamic_map_parameters() +{ + auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return false; + } + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + return true; +} + +void GpioCommandController::store_command_interface_types() +{ + for (const auto & entry : params_.command_interfaces.gpios_map) + { + const auto & gpio_name = entry.first; + const auto & interfaces = entry.second; + + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(command_interface_types_), + [gpio_name](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +void GpioCommandController::store_state_interface_types() +{ + for (const auto & entry : params_.state_interfaces.gpios_map) + { + const auto & gpio_name = entry.first; + const auto & interfaces = entry.second; + + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(state_interface_types_), + [gpio_name](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +void GpioCommandController::initialize_gpio_state_msg() +{ + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + gpio_state_msg.interface_groups.resize(params_.gpios.size()); + gpio_state_msg.interface_values.resize(params_.gpios.size()); + + for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) + { + const auto gpio_name = params_.gpios[gpio_index]; + gpio_state_msg.interface_groups[gpio_index] = gpio_name; + gpio_state_msg.interface_values[gpio_index].interface_names = + get_gpios_state_interfaces_names(gpio_name); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + gpio_state_msg.interface_values[gpio_index].interface_names.size(), + std::numeric_limits::quiet_NaN()); + } +} + +InterfacesNames GpioCommandController::get_gpios_state_interfaces_names( + const std::string & gpio_name) const +{ + InterfacesNames result; + for (const auto & interface_name : state_interface_types_) + { + const auto it = state_interfaces_map_.find(interface_name); + if (it != state_interfaces_map_.cend() && it->second.get().get_prefix_name() == gpio_name) + { + result.emplace_back(it->second.get().get_interface_name()); + } + } + return result; +} + +template +std::unordered_map> +GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces) +{ + std::unordered_map> map; + for (const auto & interface_name : interfaces_from_params) + { + auto interface = std::find_if( + configured_interfaces.begin(), configured_interfaces.end(), + [&](const auto & configured_interface) + { + const auto full_name_interface_name = configured_interface.get_name(); + return full_name_interface_name == interface_name; + }); + if (interface != configured_interfaces.end()) + { + map.emplace(interface_name, std::ref(*interface)); + } + } + return map; +} + +template +bool GpioCommandController::check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces) +{ + if (!(configured_interfaces.size() == interfaces_from_params.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + configured_interfaces.size()); + for (const auto & interface : interfaces_from_params) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %s", interface.c_str()); + } + print_interface(get_node()->get_logger(), configured_interfaces); + return false; + } + return true; +} + +controller_interface::return_type GpioCommandController::update_gpios_commands() +{ + auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); + if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + { + return controller_interface::return_type::OK; + } + + const auto gpio_commands = *(*gpio_commands_ptr); + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + { + const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + if ( + gpio_commands.interface_values[gpio_index].values.size() != + gpio_commands.interface_values[gpio_index].interface_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "For gpio %s interfaces_names do not match values", + gpio_name.c_str()); + return controller_interface::return_type::ERROR; + } + for (std::size_t command_interface_index = 0; + command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + ++command_interface_index) + { + apply_command(gpio_commands, gpio_index, command_interface_index); + } + } + return controller_interface::return_type::OK; +} + +void GpioCommandController::apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const +{ + const auto full_command_interface_name = + gpio_commands.interface_groups[gpio_index] + '/' + + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage of %s with message: %s \n", + full_command_interface_name.c_str(), e.what()); + } +} + +void GpioCommandController::update_gpios_states() +{ + if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + { + return; + } + + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + ++gpio_index) + { + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) + { + apply_state_value(gpio_state_msg, gpio_index, interface_index); + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); +} + +void GpioCommandController::apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const +{ + const auto interface_name = + state_msg.interface_groups[gpio_index] + '/' + + state_msg.interface_values[gpio_index].interface_names[interface_index]; + try + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(interface_name).get().get_value(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during reading state of: %s \n", interface_name.c_str()); + } +} + +} // namespace gpio_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_controllers::GpioCommandController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml new file mode 100644 index 0000000000..0c6aade35f --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,34 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + read_only: true, + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of command interfaces for each gpio.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } + + state_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of state interfaces for each gpio. If empty all available gpios' states are used.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp new file mode 100644 index 0000000000..14ff4e6a27 --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,605 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; + +namespace +{ +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); +}; + +class GpioCommandControllerTestSuite : public ::testing::Test +{ +public: + GpioCommandControllerTestSuite() + { + rclcpp::init(0, nullptr); + node = std::make_unique("node"); + } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + void setup_command_and_state_interfaces() + { + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + } + + void move_to_activate_state(controller_interface::return_type result_of_initialization) + { + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) + { + ASSERT_GE(max_sub_check_loop_count, 0) + << "Test was unable to publish a message through controller/broadcaster update loop"; + } + + void assert_default_command_and_state_values() + { + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); + } + + void update_controller_loop() + { + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + CmdType createGpioCommand( + const std::vector & gpios_names, + const std::vector & interface_values) + { + CmdType command; + command.interface_groups = gpios_names; + command.interface_values = interface_values; + return command; + } + + control_msgs::msg::InterfaceValue createInterfaceValue( + const std::vector & interfaces_names, + const std::vector & interfaces_values) + { + control_msgs::msg::InterfaceValue output; + output.interface_names = interfaces_names; + output.values = interfaces_values; + return output; + } + + void wait_one_miliseconds_for_timeout() + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller.get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + int wait_for_subscription( + std::shared_ptr subscription, int max_sub_check_loop_count = 5) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + return max_sub_check_loop_count; + } + + FriendGpioCommandController controller; + + const std::vector gpio_names{"gpio1", "gpio2"}; + std::vector gpio_commands{1.0, 0.0, 3.1}; + std::vector gpio_states{1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; + CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; + CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; + + StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; + StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; + StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; + std::unique_ptr node; +}; + +TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) +{ + const auto result = controller.init("test_gpio_command_controller", ""); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldNotFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldNotFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGpiosAreSetAndInterfacesAreSetForAllGpiosThenInitShouldSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + const auto result = controller.init("test_gpio_command_controller", "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandAndStateInterfacesAreEmptyAndNoInterfacesAreSetInUrdfConfigurationShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedStateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandInterfacesDontMatchStatesButBothMatchAssignedOnesThenOnActivationShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + assert_default_command_and_state_values(); + update_controller_loop(); + assert_default_command_and_state_values(); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, + {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + const auto command = createGpioCommand( + {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), + createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + const auto command = + createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), + createInterfaceValue({"ana.1"}, {21.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + auto command_pub = node->create_publisher( + std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + command_pub->publish(command); + wait_one_miliseconds_for_timeout(); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", node_options)); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(1), "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} + +TEST_F( + GpioCommandControllerTestSuite, + ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = + controller.init("test_gpio_command_controller", "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp new file mode 100644 index 0000000000..491c13c6b3 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -0,0 +1,37 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + controller_manager::ControllerManager control_mananger( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(control_mananger.load_controller( + "test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + rclcpp::shutdown(); +} diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e11321aefe..94ead71569 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,153 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- +* Replace RCLCPP\_*_STREAM macros with RCLCPP\_* (backport `#1600 `_) (`#1602 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Bump version of pre-commit hooks (backport `#1514 `_) (`#1515 `_) +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Add explicit cast to period.count() (`#1404 `_) (`#1405 `_) +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- +* Add test for effort gripper controller (`#769 `_) (`#867 `_) +* Contributors: mergify[bot] + +2.28.0 (2023-11-30) +------------------- +* Fixed implementation so that effort_controllers/GripperActionController works. (`#756 `_) (`#868 `_) +* Contributors: mergify[bot] + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* [JTC] Fix missing parameter deprecation warnings (`#630 `_) +* Contributors: Noel Jiménez García, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix compilation warnings (`#621 `_) (`#623 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich, Mathias Lüdtke, Noel Jiménez García + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- +* Fix Segfault in GripperActionController (`#527 `_) (`#530 `_) +* Contributors: Erik Holum, Bence Magyar + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- +* Changing to_chrono to use nanoseconds (`#507 `_) (`#509 `_) +* Contributors: Dan Wahl + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- * Add basic gripper controller tests (`#459 `_) diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 97ccd2788d..8edaaf6386 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -1,34 +1,29 @@ -cmake_minimum_required(VERSION 3.5) -project(gripper_controllers) +cmake_minimum_required(VERSION 3.16) +project(gripper_controllers LANGUAGES CXX) if(APPLE OR WIN32) message(WARNING "gripper controllers are not available on OSX or Windows") return() endif() -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_action - realtime_tools + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -40,14 +35,16 @@ generate_parameter_library(gripper_action_controller_parameters add_library(gripper_action_controller SHARED src/gripper_action_controller.cpp ) -ament_target_dependencies(gripper_action_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_include_directories(gripper_action_controller - PRIVATE $ - PRIVATE $ +target_compile_features(gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(gripper_action_controller PUBLIC + $ + $ ) -target_link_libraries(gripper_action_controller +target_link_libraries(gripper_action_controller PUBLIC gripper_action_controller_parameters ) +ament_target_dependencies(gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) @@ -55,8 +52,7 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_gripper_action_controllers + ament_add_gmock(test_load_gripper_action_controllers test/test_load_gripper_action_controllers.cpp ) ament_target_dependencies(test_load_gripper_action_controllers @@ -64,11 +60,9 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock( - test_gripper_controllers + ament_add_gmock(test_gripper_controllers test/test_gripper_controllers.cpp ) - target_include_directories(test_gripper_controllers PRIVATE include) target_link_libraries(test_gripper_controllers gripper_action_controller ) @@ -76,9 +70,8 @@ endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/gripper_action_controller ) - install( TARGETS gripper_action_controller diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..7f51c8f4ac --- /dev/null +++ b/gripper_controllers/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gripper_controllers/doc/userdoc.rst + +.. _gripper_controllers_userdoc: + +Gripper Action Controller +-------------------------------- + +Controllers for executing a gripper command action for simple single-dof grippers: + +- ``position_controllers/GripperActionController`` +- ``effort_controllers/GripperActionController`` + +Parameters +^^^^^^^^^^^ +These controllers use the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/gripper_action_controller_parameters.yaml diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 830de7b514..530df2f218 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -37,13 +37,15 @@ #include "gripper_controllers/visibility_control.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project -#include "gripper_action_controller_parameters.hpp" #include "gripper_controllers/hardware_interface_adapter.hpp" +// auto-generated by generate_parameter_library +#include "gripper_controllers/gripper_action_controller_parameters.hpp" + namespace gripper_action_controller { /** @@ -104,7 +106,7 @@ class GripperActionController : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is re-used to set the realtime buffer + // pre-allocated memory that is reused to set the realtime buffer Commands command_struct_, command_struct_rt_; protected: @@ -115,6 +117,7 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; using HwIfaceAdapter = HardwareInterfaceAdapter; @@ -123,7 +126,7 @@ class GripperActionController : public controller_interface::ControllerInterface bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. std::optional> - joint_position_command_interface_; + joint_command_interface_; std::optional> joint_position_state_interface_; std::optional> @@ -134,7 +137,8 @@ class GripperActionController : public controller_interface::ControllerInterface HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. - RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index e28495fd1e..45b4142b1f 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -28,11 +28,12 @@ template void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - if (rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { // Marks the current goal as canceled - rt_active_goal_->setCanceled(std::make_shared()); - rt_active_goal_.reset(); + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } @@ -86,29 +87,31 @@ template void GripperActionController::accepted_callback( std::shared_ptr goal_handle) // Try to update goal { - { - auto rt_goal = std::make_shared(goal_handle); + auto rt_goal = std::make_shared(goal_handle); - // Accept new goal - preempt_active_goal(); + // Accept new goal + preempt_active_goal(); - // This is the non-realtime command_struct - // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; - command_.writeFromNonRT(command_struct_); + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_ = goal_handle->get_goal()->command.position; + command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + command_.writeFromNonRT(command_struct_); - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = false; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); - last_movement_time_ = get_node()->now(); - rt_active_goal_ = rt_goal; - rt_active_goal_->execute(); - } // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } template @@ -118,7 +121,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode set_hold_position(); @@ -128,9 +132,9 @@ rclcpp_action::CancelResponse GripperActionController::cancel // Mark the current goal as canceled auto action_res = std::make_shared(); - rt_active_goal_->setCanceled(action_res); + active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -148,7 +152,8 @@ void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - if (!rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) { return; } @@ -160,8 +165,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); - rt_active_goal_->setSucceeded(pre_alloc_result_); - rt_active_goal_.reset(); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } else { @@ -179,14 +184,14 @@ void GripperActionController::check_for_success( if (params_.allow_stalling) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); - rt_active_goal_->setSucceeded(pre_alloc_result_); + active_goal->setSucceeded(pre_alloc_result_); } else { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); - rt_active_goal_->setAborted(pre_alloc_result_); + active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } } @@ -205,8 +210,8 @@ controller_interface::CallbackReturn GripperActionController: // Action status checking update rate action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); - RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + RCLCPP_INFO( + logger, "Action status changes will be monitored at %f Hz.", params_.action_monitor_rate); // Controlled joint if (params_.joint.empty()) @@ -221,21 +226,20 @@ template controller_interface::CallbackReturn GripperActionController::on_activate( const rclcpp_lifecycle::State &) { - auto position_command_interface_it = std::find_if( + auto command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [](const hardware_interface::LoanedCommandInterface & command_interface) - { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); - if (position_command_interface_it == command_interfaces_.end()) + { return command_interface.get_interface_name() == HardwareInterface; }); + if (command_interface_it == command_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 %s command interface", HardwareInterface); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_prefix_name() != params_.joint) + if (command_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Position command interface is different than joint name `" - << position_command_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), "Command interface is different than joint name `%s` != `%s`", + command_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( @@ -249,10 +253,10 @@ controller_interface::CallbackReturn GripperActionController: } if (position_state_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Position state interface is different than joint name `" - << position_state_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), + "Position state interface is different than joint name `%s` != `%s`", + position_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( @@ -266,19 +270,19 @@ controller_interface::CallbackReturn GripperActionController: } if (velocity_state_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Velocity command interface is different than joint name `" - << velocity_state_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), + "Velocity command interface is different than joint name `%s` != `%s`", + velocity_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } - joint_position_command_interface_ = *position_command_interface_it; + joint_command_interface_ = *command_interface_it; joint_position_state_interface_ = *position_state_interface_it; joint_velocity_state_interface_ = *velocity_state_interface_it; // Hardware interface adapter - hw_iface_adapter_.init(joint_position_command_interface_, get_node()); + hw_iface_adapter_.init(joint_command_interface_, get_node()); // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); @@ -306,7 +310,7 @@ template controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { - joint_position_command_interface_ = std::nullopt; + joint_command_interface_ = std::nullopt; joint_position_state_interface_ = std::nullopt; joint_velocity_state_interface_ = std::nullopt; release_interfaces(); @@ -319,7 +323,7 @@ GripperActionController::command_interface_configuration() co { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {params_.joint + "/" + hardware_interface::HW_IF_POSITION}}; + {params_.joint + "/" + HardwareInterface}}; } template diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 97ebbb1f4b..a4b82bd25f 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -131,7 +131,7 @@ class HardwareInterfaceAdapter { joint_handle_ = joint_handle; // Init PID gains from ROS parameter server - const std::string prefix = "gains." + joint_handle_->get().get_name(); + const std::string prefix = "gains." + joint_handle_->get().get_prefix_name(); const auto k_p = auto_declare(node, prefix + ".p", 0.0); const auto k_i = auto_declare(node, prefix + ".i", 0.0); const auto k_d = auto_declare(node, prefix + ".d", 0.0); @@ -166,7 +166,8 @@ class HardwareInterfaceAdapter // Time since the last call to update const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs - double command = pid_->computeCommand(error_position, error_velocity, period.count()); + double command = + pid_->computeCommand(error_position, error_velocity, static_cast(period.count())); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index b033e99b12..0f75b7a6c6 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,29 +4,39 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.15.0 + 2.44.0 The gripper_controllers package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Jafar Abdi ament_cmake + backward_ros control_msgs control_toolbox controller_interface generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_action realtime_tools - pluginlib - ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml index 9027e5b730..a847643b9f 100644 --- a/gripper_controllers/src/gripper_action_controller_parameters.yaml +++ b/gripper_controllers/src/gripper_action_controller_parameters.yaml @@ -4,7 +4,7 @@ gripper_action_controller: default_value: 20.0, description: "Hz", validation: { - lower_bounds: [0.1] + gt_eq: [0.1] }, } joint: { @@ -15,7 +15,7 @@ gripper_action_controller: type: double, default_value: 0.01, validation: { - lower_bounds: [0.0] + gt_eq: [0.0] }, } max_effort: { @@ -23,7 +23,7 @@ gripper_action_controller: default_value: 0.0, description: "Max allowable effort", validation: { - lower_bounds: [0.0] + gt_eq: [0.0] }, } allow_stalling: { diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 4c82eb6961..6963dfec93 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -31,124 +31,155 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using GripperCommandAction = control_msgs::action::GripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; -void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +template +void GripperControllerTest::SetUpTestCase() +{ + rclcpp::init(0, nullptr); +} -void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } +template +void GripperControllerTest::TearDownTestCase() +{ + rclcpp::shutdown(); +} -void GripperControllerTest::SetUp() +template +void GripperControllerTest::SetUp() { // initialize controller - controller_ = std::make_unique(); + controller_ = std::make_unique>(); } -void GripperControllerTest::TearDown() { controller_.reset(nullptr); } +template +void GripperControllerTest::TearDown() +{ + controller_.reset(nullptr); +} -void GripperControllerTest::SetUpController() +template +void GripperControllerTest::SetUpController() { const auto result = controller_->init("gripper_controller"); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } -TEST_F(GripperControllerTest, ParametersNotSet) +using TestTypes = ::testing::Types< + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(GripperControllerTest, TestTypes); + +TYPED_TEST(GripperControllerTest, ParametersNotSet) { - SetUpController(); + this->SetUpController(); // configure failed, 'joints' parameter not set ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, JointParameterIsEmpty) +TYPED_TEST(GripperControllerTest, JointParameterIsEmpty) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", ""}); + this->controller_->get_node()->set_parameter({"joint", ""}); // configure failed, 'joints' is empty ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, ConfigureParamsSuccess) +TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint_1"}); + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); // configure successful ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + TypeParam::value)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, ActivateSuccess) +TYPED_TEST(GripperControllerTest, ActivateSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); // activate successful ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } -TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +TYPED_TEST(GripperControllerTest, ActivateDeactivateActivateSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_deactivate(rclcpp_lifecycle::State()), + this->controller_->on_deactivate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); // re-assign interfaces std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 3e9750a603..4983c8102d 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -26,17 +26,22 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using hardware_interface::StateInterface; +namespace +{ // subclassing and friending so we can access member variables +template class FriendGripperController -: public gripper_action_controller::GripperActionController +: public gripper_action_controller::GripperActionController { FRIEND_TEST(GripperControllerTest, CommandSuccessTest); }; +template class GripperControllerTest : public ::testing::Test { public: @@ -50,7 +55,7 @@ class GripperControllerTest : public ::testing::Test void SetUpHandles(); protected: - std::unique_ptr controller_; + std::unique_ptr> controller_; // dummy joint state values used for tests const std::string joint_name_ = "joint1"; @@ -59,7 +64,9 @@ class GripperControllerTest : public ::testing::Test StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; + CommandInterface joint_1_cmd_{joint_name_, T::value, &joint_commands_[0]}; }; +} // anonymous namespace + #endif // TEST_GRIPPER_CONTROLLERS_HPP_ diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index b355cdf34a..0ef5f0bcb2 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadGripperActionControllers, load_controller) @@ -32,10 +34,14 @@ TEST(TestLoadGripperActionControllers, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_gripper_action_position_controller", "position_controllers/GripperActionController")); - ASSERT_NO_THROW(cm.load_controller( - "test_gripper_action_effort_controller", "effort_controllers/GripperActionController")); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController"), + nullptr); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + nullptr); rclcpp::shutdown(); } diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 0f26b4cbf2..31362e970b 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,145 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Broadcaster parameters (`#650 `_) (`#678 `_) +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Christoph Fröhlich, Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index e49fb65ad3..3b09d9e72e 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -1,28 +1,23 @@ -cmake_minimum_required(VERSION 3.5) -project(imu_sensor_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(imu_sensor_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - sensor_msgs + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -31,21 +26,22 @@ generate_parameter_library(imu_sensor_broadcaster_parameters src/imu_sensor_broadcaster_parameters.yaml ) -add_library( - imu_sensor_broadcaster SHARED +add_library(imu_sensor_broadcaster SHARED src/imu_sensor_broadcaster.cpp ) -target_include_directories(imu_sensor_broadcaster - PRIVATE $ - PRIVATE $ +target_compile_features(imu_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(imu_sensor_broadcaster PUBLIC + $ + $ +) +target_link_libraries(imu_sensor_broadcaster PUBLIC + imu_sensor_broadcaster_parameters ) -ament_target_dependencies(imu_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(imu_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") -target_link_libraries(imu_sensor_broadcaster - imu_sensor_broadcaster_parameters -) pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) @@ -56,21 +52,22 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_imu_sensor_broadcaster + add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_imu_sensor_broadcaster + imu_sensor_broadcaster + ) ament_target_dependencies(test_load_imu_sensor_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_imu_sensor_broadcaster + add_rostest_with_parameters_gmock(test_imu_sensor_broadcaster test/test_imu_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster imu_sensor_broadcaster @@ -78,18 +75,12 @@ if(BUILD_TESTING) ament_target_dependencies(test_imu_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_imu_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets - ) endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/imu_sensor_broadcaster ) - install( TARGETS imu_sensor_broadcaster diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 90704c8504..09b51a7ecb 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/imu_sensor_broadcaster/doc/userdoc.rst + .. _imu_sensor_broadcaster_userdoc: IMU Sensor Broadcaster @@ -9,10 +11,17 @@ The controller is a wrapper around ``IMUSensor`` semantic component (see ``contr Parameters ^^^^^^^^^^^ -frame_id (mandatory) - Frame in which the output message will be published. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/imu_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: -sensor_name (mandatory) - Defines sensor name used as prefix for its interfaces. - Interface names are: /orientation.x, ..., /angular_velocity.x, ..., - /linear_acceleration.x. +.. literalinclude:: ../test/imu_sensor_broadcaster_params.yaml + :language: yaml diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index d78e28780e..cdbf4490a0 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -25,12 +25,13 @@ #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" -#include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/imu_sensor.hpp" #include "sensor_msgs/msg/imu.hpp" +// auto-generated by generate_parameter_library +#include "imu_sensor_broadcaster/imu_sensor_broadcaster_parameters.hpp" namespace imu_sensor_broadcaster { diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 79eb5e14a7..44dae51a0d 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,15 +2,25 @@ imu_sensor_broadcaster - 2.15.0 + 2.44.0 Controller to publish readings of IMU sensors. + Bence Magyar - Denis Štogl - Victor Lopez + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Victor Lopez + ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface @@ -24,7 +34,7 @@ ament_lint_auto ament_lint_common controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 3e2d574f0f..04a28e5368 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -44,17 +44,6 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - if (params_.sensor_name.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); - return CallbackReturn::ERROR; - } - - if (params_.frame_id.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); - return CallbackReturn::ERROR; - } imu_sensor_ = std::make_unique( semantic_components::IMUSensor(params_.sensor_name)); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index c0daba9f11..d45bf8583b 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -2,12 +2,20 @@ imu_sensor_broadcaster: sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Defines sensor name used as prefix for its interfaces. + Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., + /linear_acceleration.x.``", + validation: { + not_empty<>: null + } } frame_id: { type: string, default_value: "", description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } } static_covariance_orientation: { type: double_array, diff --git a/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..c1c660d2c4 --- /dev/null +++ b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml @@ -0,0 +1,5 @@ +test_imu_sensor_broadcaster: + ros__parameters: + + sensor_name: "imu_sensor" + frame_id: "imu_sensor_frame" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 31c8d9e2f0..8a97f8a4cd 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -40,9 +42,9 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void IMUSensorBroadcasterTest::SetUpTestCase() {} -void IMUSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void IMUSensorBroadcasterTest::TearDownTestCase() {} void IMUSensorBroadcasterTest::SetUp() { @@ -75,61 +77,39 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) { // create a new subscriber + sensor_msgs::msg::Imu::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_imu_sensor_broadcaster/imu", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(imu_msg, msg_info)); -} - -TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) -{ - SetUpIMUBroadcaster(); - - // configure failed - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(IMUSensorBroadcasterTest, SensorName_NotSet) -{ - SetUpIMUBroadcaster(); - - // set the 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - - // configure failed, 'sensor_name' parameter not set - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(IMUSensorBroadcasterTest, FrameId_NotSet) -{ - SetUpIMUBroadcaster(); - - // set the 'sensor_name' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + imu_msg = *received_msg; } TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) @@ -144,6 +124,14 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -157,6 +145,25 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // deactivate passed + ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) @@ -208,3 +215,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) EXPECT_EQ(imu_msg.linear_acceleration_covariance[i], 0.0); } } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index e90c9915ed..f4e6105ed6 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -28,8 +28,6 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -38,8 +36,17 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster")); + ASSERT_NE( + cm.load_controller( + "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), + nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 68d25c05aa..fdf4e92f6b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,149 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) (`#1339 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- +* joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) (`#863 `_) +* Contributors: mergify[bot] + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- +* Fix docs format (`#591 `_) +* Contributors: Christoph Fröhlich + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 0717577e15..cadc96b4e3 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -1,36 +1,11 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_state_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(joint_state_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) -endif() - -find_package(ament_cmake REQUIRED) -find_package(control_msgs REQUIRED) -find_package(controller_interface REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rcutils REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(sensor_msgs REQUIRED) - -generate_parameter_library(joint_state_broadcaster_parameters - src/joint_state_broadcaster_parameters.yaml -) - -add_library(joint_state_broadcaster - SHARED - src/joint_state_broadcaster.cpp -) -target_include_directories(joint_state_broadcaster PRIVATE include) -ament_target_dependencies(joint_state_broadcaster +set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces control_msgs controller_interface @@ -41,27 +16,34 @@ ament_target_dependencies(joint_state_broadcaster realtime_tools sensor_msgs ) -target_link_libraries(joint_state_broadcaster + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(joint_state_broadcaster_parameters + src/joint_state_broadcaster_parameters.yaml +) + +add_library(joint_state_broadcaster SHARED + src/joint_state_broadcaster.cpp +) +target_compile_features(joint_state_broadcaster PUBLIC cxx_std_17) +target_include_directories(joint_state_broadcaster PUBLIC + $ + $ +) +target_link_libraries(joint_state_broadcaster PUBLIC joint_state_broadcaster_parameters ) +ament_target_dependencies(joint_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - joint_state_broadcaster - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -69,38 +51,43 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_state_broadcaster + ament_add_gmock(test_load_joint_state_broadcaster test/test_load_joint_state_broadcaster.cpp ) - target_include_directories(test_load_joint_state_broadcaster PRIVATE include) + target_link_libraries(test_load_joint_state_broadcaster + joint_state_broadcaster + ) ament_target_dependencies(test_load_joint_state_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_joint_state_broadcaster + ament_add_gmock(test_joint_state_broadcaster test/test_joint_state_broadcaster.cpp ) - target_include_directories(test_joint_state_broadcaster PRIVATE include) target_link_libraries(test_joint_state_broadcaster joint_state_broadcaster ) + ament_target_dependencies(test_joint_state_broadcaster + hardware_interface + ) endif() -ament_export_dependencies( - control_msgs - controller_interface - generate_parameter_library - rclcpp_lifecycle - sensor_msgs -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/joint_state_broadcaster ) -ament_export_libraries( - joint_state_broadcaster +install( + TARGETS + joint_state_broadcaster + joint_state_broadcaster_parameters + EXPORT export_joint_state_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_joint_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml new file mode 100644 index 0000000000..c96f8301ae --- /dev/null +++ b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml @@ -0,0 +1,46 @@ +map_interface_to_joint_state: | + Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. + Usecases: + + #. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. + Typically one would map both values in separate interfaces in the framework. + To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. + #. A robot provides multiple measuring techniques for its joint values which results in slightly different values. + Typically one would use separate interface for providing those values in the framework. + Using multiple joint_state_broadcaster instances we could publish and show both in RViz. + + Format (each line is optional): + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: + \t\tvelocity: + \t\teffort: + + + Examples: + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: kf_estimated_position + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tvelocity: derived_velocity + \t\teffort: derived_effort + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: torque_sensor + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: current_sensor diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 8a030d9356..c7bf4fa9a1 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_state_broadcaster/doc/userdoc.rst + .. _joint_state_broadcaster_userdoc: joint_state_broadcaster @@ -20,70 +22,19 @@ If none of the requested interface are not defined, the controller returns error Parameters ---------- - -``use_local_topics`` - Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. - - -``joints`` - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``interfaces`` parameter. - Joint state broadcaster asks for access to all defined interfaces on all defined joints. - - -``interfaces`` - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``joints`` parameter. - - -``extra_joints`` - Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. - - -``map_interface_to_joint_state`` - Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. - Usecases: - - 1. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. - Typically one would map both values in separate interfaces in the framework. - To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. - - 1. A robot provides multiple measuring techniques for its joint values which results in slightly different values. - Typically one would use separate interface for providing those values in the framework. - Using multiple joint_state_broadcaster instances we could publish and show both in RViz. - - Format (each line is optional): - - .. code-block:: yaml - - map_interface_to_joint_state: - position: - velocity: - effort: - - - Examples: - - .. code-block:: yaml - - map_interface_to_joint_state: - position: kf_estimated_position - - - .. code-block:: yaml - - map_interface_to_joint_state: - velocity: derived_velocity - effort: derived_effort +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. - .. code-block:: yaml +List of parameters +,,,,,,,,,,,,,,,,,, - map_interface_to_joint_state: - effort: torque_sensor +.. generate_parameter_library_details:: + ../src/joint_state_broadcaster_parameters.yaml + joint_state_broadcaster_parameter_context.yml - .. code-block:: yaml +An example parameter file +,,,,,,,,,,,,,,,,,,,,,,,,, - map_interface_to_joint_state: - effort: current_sensor +.. generate_parameter_library_default:: + ../src/joint_state_broadcaster_parameters.yaml diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 4761f3f250..70ab9fd489 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -23,12 +23,14 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "joint_state_broadcaster/visibility_control.h" -#include "joint_state_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" +// auto-generated by generate_parameter_library +#include "joint_state_broadcaster/joint_state_broadcaster_parameters.hpp" + namespace joint_state_broadcaster { /** diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c24d274a93..4fbfddc11e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,33 +1,40 @@ joint_state_broadcaster - 2.15.0 + 2.44.0 Broadcaster to publish joint state + Bence Magyar - Denis Stogl - Jordan Palacios - Karsten Knese + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 - ament_cmake + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ - pluginlib - rcutils + Jordan Palacios + Karsten Knese - pluginlib - rcutils + ament_cmake + backward_ros + builtin_interfaces control_msgs controller_interface generate_parameter_library - hardware_interface + pluginlib rclcpp_lifecycle + rcutils realtime_tools sensor_msgs ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 74a19c9ed0..dd499ae254 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -202,6 +202,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { joint_names_.clear(); + name_if_value_mapping_.clear(); return CallbackReturn::SUCCESS; } @@ -297,6 +298,8 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; + dynamic_joint_state_msg.joint_names.clear(); + dynamic_joint_state_msg.interface_values.clear(); for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index ba0d4f0051..8f0d4da6c5 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -2,18 +2,29 @@ joint_state_broadcaster: use_local_topics: { type: bool, default_value: false, + description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." } joints: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``interfaces`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published. + Joint state broadcaster asks for access to all defined interfaces on all defined joints." } extra_joints: { type: string_array, default_value: [], + description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0." } interfaces: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``joints`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published." } map_interface_to_joint_state: position: { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 7f95e232e2..c91a89c110 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -163,7 +163,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); } -TEST_F(JointStateBroadcasterTest, ActivateTest) +TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) { // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); @@ -177,6 +177,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTest) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -205,6 +213,102 @@ TEST_F(JointStateBroadcasterTest, ActivateTest) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + + SetUpStateBroadcaster(); + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // Now deactivate and activate with only 2 set of joints and interfaces (to create as in one of + // the interface is unavailable) + ASSERT_EQ(state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + const std::vector JOINT_NAMES = {"joint1", "joint2"}; + assign_state_interfaces(JOINT_NAMES, interface_names_); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS_WITH_ONE_DEACTIVATED = JOINT_NAMES.size(); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + + // dynamic joint state initialized + const auto & new_dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) { const std::vector JOINT_NAMES = {}; @@ -218,6 +322,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -259,6 +371,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -287,7 +407,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ElementsAreArray(interface_names_)); } -TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) +TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) { const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0]}; @@ -300,6 +420,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -329,6 +457,18 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); ASSERT_THAT( dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); + + // deactivate + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT( + state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -344,6 +484,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -412,6 +560,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -455,6 +611,14 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, SizeIs(0)); @@ -492,6 +656,14 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); @@ -531,19 +703,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) state_broadcaster_->get_node()->set_parameter( {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); - // configure ok - ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message("joint_states", joint_state_msg); const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_); @@ -585,7 +750,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) controller_interface::return_type::OK); } -void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +void JointStateBroadcasterTest::activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg) { auto node_state = state_broadcaster_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -593,32 +759,47 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + sensor_msgs::msg::JointState::SharedPtr received_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (!received_msg && test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription + joint_state_msg = *received_msg; +} + +void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +{ sensor_msgs::msg::JointState joint_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); + activate_and_get_joint_state_message(topic, joint_state_msg); const size_t NUM_JOINTS = joint_names_.size(); ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS)); @@ -655,51 +836,58 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) {}; + auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) + { dynamic_joint_state_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + dynamic_joint_state_msg.reset(); + ASSERT_FALSE(dynamic_joint_state_msg); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (dynamic_joint_state_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; - - // take message from subscription - control_msgs::msg::DynamicJointState dynamic_joint_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); + ASSERT_TRUE(dynamic_joint_state_msg); const size_t NUM_JOINTS = 3; const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; - ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS)); // the order in the message may be different // we only check that all values in this test are present in the message // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(); ++i) + for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) { ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].interface_names, + dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); - const auto index_in_original_order = std::distance( + const auto index_in_original_order = static_cast(std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg.joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].values, + dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); } } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..dd6f7d5b2f 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -33,10 +33,11 @@ using hardware_interface::HW_IF_VELOCITY; class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTest); + FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); + FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface); + FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); @@ -71,6 +72,9 @@ class JointStateBroadcasterTest : public ::testing::Test void test_published_dynamic_joint_state_message(const std::string & topic); + void activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & msg); + protected: // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index d7a2934f57..5efb587805 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -34,8 +34,10 @@ TEST(TestLoadJointStateBroadcaster, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_state_broadcaster", "joint_state_broadcaster/JointStateBroadcaster")); + ASSERT_NE( + cm.load_controller( + "test_joint_state_broadcaster", "joint_state_broadcaster/JointStateBroadcaster"), + nullptr); rclcpp::shutdown(); } diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index bc59ab4960..6f440fd32c 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,234 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Replace RCLCPP\_*_STREAM macros with RCLCPP\_* (backport `#1600 `_) (`#1602 `_) +* [jtc tests] avoid dangling ref of command / state interfaces (backport `#1596 `_) (`#1597 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- +* Use constructor parameters instead of initializer list (backport `#1587 `_) (`#1589 `_) +* Contributors: Felix Exner (fexner) + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* [JTC]: Abort goal on deactivate (backport `#1517 `_) (`#1521 `_) +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Remove empty on_shutdown() callbacks (backport `#1477 `_) (`#1482 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- +* Increase margin for state_publish_rate (`#1430 `_) +* Add an error msg if empty message is received (backport `#1424 `_) (`#1428 `_) +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* [JTC] Fix the JTC length_error exceptions in the tests (backport `#1360 `_) (`#1361 `_) +* [jtc] Improve trajectory sampling efficiency (`#1297 `_) (`#1357 `_) +* fixes for windows compilation (`#1330 `_) (`#1332 `_) +* [JTC] Add Parameter to Toggle State Setting on Activation (backport `#1231 `_) (`#1320 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- +* Fix admittance controller interface read/write logic (backport `#1232 `_) (`#1234 `_) +* Contributors: mergify[bot] + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (backport `#1192 `_ + `#1209 `_) (`#1208 `_) +* [JTC] Process tolerances sent with action goal (backport `#716 `_) (`#1189 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* Still fill desired/actual deprecated fields (`#1172 `_) +* JTC trajectory end time validation fix (`#1090 `_) (`#1140 `_) +* Contributors: Bence Magyar, mergify[bot] + +2.35.0 (2024-05-22) +------------------- +* [JTC] Remove unused test code (`#1095 `_) (`#1096 `_) +* Contributors: mergify[bot] + +2.34.0 (2024-04-01) +------------------- +* Remove action_msg dependency (`#1077 `_) (`#1080 `_) +* [JTC] Angle wraparound for first segment of trajectory (`#796 `_) (`#1033 `_) +* Bump version of pre-commit hooks (`#1073 `_) (`#1074 `_) +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Fix usage of M_PI on Windows (`#1036 `_) (`#1037 `_) +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* [JTC] Fill action error_strings (`#887 `_) (`#1009 `_) +* [JTC] Invalidate empty trajectory messages (`#902 `_) (`#1000 `_) +* Revert "[JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_)" (`#978 `_) (`#986 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Cleanup package.xml und clarify tests of JTC. (backport `#889 `_) (`#924 `_) +* [JTC] Remove deprecation from parameters validation file. (`#476 `_) (`#926 `_) +* [JTC] Cancel goal in on_deactivate (`#962 `_) (`#970 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- +* [JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_) (`#968 `_) +* [JTC] Add console output for tolerance checks (backport `#932 `_) (`#938 `_) +* [JTC] Cleanup includes (`#943 `_) (`#959 `_) +* Fix whitespace +* Add rqt_JTC to docs (`#950 `_) (`#952 `_) +* Contributors: Bence Magyar, mergify[bot] + +2.30.0 (2023-12-20) +------------------- +* Fix floating point comparison in JTC (backport `#879 `_) +* [JTC] Continue with last trajectory-point on success (backport `#842 `_) +* [JTC] Remove start_with_holding option (backport `#839 `_) +* [JTC] Activate checks for parameter validation backport (`#857 `_) +* [JTC] Improve update methods for tests (backport `#858 `_) +* [JTC] Fix dynamic reconfigure of tolerances (backport `#849 `_) +* [JTC] Remove unused home pose (backport `#845 `_) +* [JTC] Activate update of dynamic parameters (backport `#761 `_) +* [JTC] Fix tests when state offset is used (backport `#797 `_) +* Rename wraparound class variables (backport `#834 `_) +* Update requirements of state interfaces (backport `#798 `_) +* [JTC] Fix typos, implicit cast, const member functions (backport `#748 `_) +* Cleanup comments and unnecessary checks (backport `#803 `_) +* [JTC] Add tests for acceleration command interface (backport `#752 `_) +* [Docs] Improve interface description of JTC (backport `#770 `_) +* [JTC] Add time-out for trajectory interfaces (backport `#609 `_) +* [JTC] Fix hold position mode with goal_time>0 (backport `#758 `_) +* [JTC] Add note on goal_time=0 in docs (backport `#773 `_) +* [JTC] Make most parameters read-only (backport `#771 `_) +* [JTC] Tolerance tests + Hold on time violation (backport `#613 `_) +* [JTC] Explicitly set hold position (backport `#558 `_) +* [Doc] Fix links (backport `#715 `_) +* Contributors: Christoph Fröhlich, Dr Denis Stogl, Bence Magyar, Abishalini Sivaraman + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* [JTC] Rename parameter: normalize_error to angle_wraparound (`#772 `_) (`#776 `_) +* Remove wrong description (`#742 `_) (`#747 `_) +* [JTC] Update trajectory documentation (`#714 `_) (`#741 `_) +* Contributors: Christoph Fröhlich + +2.24.0 (2023-08-07) +------------------- +* [JTC] Disable use of closed-loop PID adapter if controller is used in open-loop mode. (`#551 `_) (`#740 `_) +* [JTC] Reject messages with effort fields (`#699 `_) (`#719 `_) (`#738 `_) +* [JTC] Re-enabling test, bugfixing and hardening. Adding a parameter to define when trajectories with non-zero velocity at the end are used. (backport `#705 `_) (`#706 `_) +* Small improvement in remapping (`#393 `_) (`#724 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Bence Magyar + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* jtc: fix minor typo in traj validation error msg (`#674 `_) (`#676 `_) +* Contributors: G.A. vd. Hoorn, Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* [JTC] Import docs from wiki.ros.org (backport `#566 `_) (`#634 `_) +* [Formatting] enable ReflowComments to also use ColumnLimit on comments (`#628 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Deprecations in generate_parameter_library. (`#616 `_) +* Remove compile warnings. (`#519 `_) (`#620 `_) +* ported the joint_trajectory_controller query_state service to ROS2 (backport `#481 `_) (`#614 `_) +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Dr. Denis, Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- +* Fix JTC from immediately returning success (`#565 `_) (`#592 `_) +* Implement new ~/controller_state message (`#578 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + +2.18.0 (2023-04-29) +------------------- +* Fix docs format (`#591 `_) +* [JTC] Configurable joint positon error normalization behavior (`#491 `_) (`#579 `_) +* Contributors: Christoph Fröhlich, Bence Magyar + +2.17.3 (2023-04-14) +------------------- +* [JTC] Add pid gain structure to documentation (`#485 `_) (`#543 `_) +* Fix markup in userdoc.rst (`#480 `_) (`#542 `_) +* Contributors: Christoph Fröhlich + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- +* fix interpolation logic (`#516 `_) (`#523 `_) +* fix JTC segfault (`#518 `_) (`#524 `_) +* Fix JTC segfault on unload (`#515 `_) (`#525 `_) +* Contributors: Solomon Wiznitzer, Márk Szitanics, Michael Wiznitzer + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 32461617ac..1d9eadd35b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -1,14 +1,8 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_trajectory_controller) +cmake_minimum_required(VERSION 3.16) +project(joint_trajectory_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -22,10 +16,13 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + rsl + tl_expected trajectory_msgs ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -35,55 +32,52 @@ generate_parameter_library(joint_trajectory_controller_parameters include/joint_trajectory_controller/validate_jtc_parameters.hpp ) -add_library(${PROJECT_NAME} SHARED +add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -target_link_libraries(${PROJECT_NAME} joint_trajectory_controller_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) +target_include_directories(joint_trajectory_controller PUBLIC + $ + $ +) +target_link_libraries(joint_trajectory_controller PUBLIC + joint_trajectory_controller_parameters +) +ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters - EXPORT export_joint_trajectory_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_trajectory test/test_trajectory.cpp) - target_include_directories(test_trajectory PRIVATE include) - target_link_libraries(test_trajectory ${PROJECT_NAME}) + target_link_libraries(test_trajectory joint_trajectory_controller) + target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + + ament_add_gmock(test_tolerances test/test_tolerances.cpp) + target_link_libraries(test_tolerances joint_trajectory_controller) + target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_trajectory_controller - test/test_trajectory_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) + test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) - target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller - ${PROJECT_NAME} - ) - ament_target_dependencies(test_trajectory_controller - ${THIS_PACKAGE_INCLUDE_DEPENDS} + joint_trajectory_controller ) + target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) - ament_add_gmock( - test_load_joint_trajectory_controller + ament_add_gmock(test_load_joint_trajectory_controller test/test_load_joint_trajectory_controller.cpp ) - target_include_directories(test_load_joint_trajectory_controller PRIVATE include) + target_link_libraries(test_load_joint_trajectory_controller + joint_trajectory_controller + ) ament_target_dependencies(test_load_joint_trajectory_controller controller_manager control_toolbox @@ -91,24 +85,29 @@ if(BUILD_TESTING) ros2_control_test_assets ) - # TODO(andyz): Disabled due to flakiness - # ament_add_gmock( - # test_trajectory_actions - # test/test_trajectory_actions.cpp - # ) - # target_include_directories(test_trajectory_actions PRIVATE include) - # target_link_libraries(test_trajectory_actions - # ${PROJECT_NAME} - # ) - # ament_target_dependencies(test_trajectory_actions - # ${THIS_PACKAGE_INCLUDE_DEPENDS} - # ) + ament_add_gmock(test_trajectory_actions + test/test_trajectory_actions.cpp + ) + set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300) + target_link_libraries(test_trajectory_actions + joint_trajectory_controller + ) endif() -ament_export_targets( - export_joint_trajectory_controller HAS_LIBRARY_TARGET + +install( + DIRECTORY include/ + DESTINATION include/joint_trajectory_controller ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install(TARGETS + joint_trajectory_controller + joint_trajectory_controller_parameters + EXPORT export_joint_trajectory_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/joint_trajectory_controller/README.md b/joint_trajectory_controller/README.md index 9bffcc3566..874176d228 100644 --- a/joint_trajectory_controller/README.md +++ b/joint_trajectory_controller/README.md @@ -2,4 +2,4 @@ The package implements controllers to interpolate joint's trajectory. -For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/). +For detailed documentation check the `docs` folder or [ros2_control documentation](https://control.ros.org/). diff --git a/joint_trajectory_controller/doc/new_trajectory.png b/joint_trajectory_controller/doc/new_trajectory.png new file mode 100644 index 0000000000..b7c07eeffe Binary files /dev/null and b/joint_trajectory_controller/doc/new_trajectory.png differ diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst new file mode 100644 index 0000000000..dbb50fcbeb --- /dev/null +++ b/joint_trajectory_controller/doc/parameters.rst @@ -0,0 +1,22 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/parameters.rst + +.. _parameters: + +Details about parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + + +List of parameters +========================= + +.. generate_parameter_library_details:: + ../src/joint_trajectory_controller_parameters.yaml + parameters_context.yaml + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/joint_trajectory_controller_parameters.yaml diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..2ffe8aed36 --- /dev/null +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -0,0 +1,13 @@ +constraints: + Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message. + +gains: | + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law + + .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. diff --git a/joint_trajectory_controller/doc/spline_acceleration.png b/joint_trajectory_controller/doc/spline_acceleration.png new file mode 100644 index 0000000000..98672beedd Binary files /dev/null and b/joint_trajectory_controller/doc/spline_acceleration.png differ diff --git a/joint_trajectory_controller/doc/spline_position.png b/joint_trajectory_controller/doc/spline_position.png new file mode 100644 index 0000000000..79167f8500 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity.png b/joint_trajectory_controller/doc/spline_position_velocity.png new file mode 100644 index 0000000000..0e959295fb Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png new file mode 100644 index 0000000000..70152eec4e Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png new file mode 100644 index 0000000000..a0f06e33c5 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png new file mode 100644 index 0000000000..a1e29de3fc Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png differ diff --git a/joint_trajectory_controller/doc/spline_velocity.png b/joint_trajectory_controller/doc/spline_velocity.png new file mode 100644 index 0000000000..d427f4da88 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_velocity.png differ diff --git a/joint_trajectory_controller/doc/spline_wrong_points.png b/joint_trajectory_controller/doc/spline_wrong_points.png new file mode 100644 index 0000000000..f73310c031 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_wrong_points.png differ diff --git a/joint_trajectory_controller/doc/trajectory.rst b/joint_trajectory_controller/doc/trajectory.rst new file mode 100644 index 0000000000..44f385de27 --- /dev/null +++ b/joint_trajectory_controller/doc/trajectory.rst @@ -0,0 +1,168 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/trajectory.rst + +.. _joint_trajectory_controller_trajectory_representation: + +Trajectory Representation +--------------------------------- + +Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. + +Currently, two interpolation methods are implemented: ``none`` and ``spline``. +By default, a spline interpolator is provided, but it's possible to support other representations. + +.. warning:: + The user has to ensure that the correct inputs are provided for the trajectory, which are needed + by the controller's setup of command interfaces and PID configuration. There is no sanity check and + missing fields in the sampled trajectory might cause segmentation faults. + +Interpolation Method ``none`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +It returns the initial point until the time for the first trajectory data point is reached. Then, it simply takes the next given datapoint. + +.. warning:: + It does not deduce (integrate) trajectory from derivatives, nor does it calculate derivatives. + I.e., one has to provide position and its derivatives as needed. + +Interpolation Method ``spline`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The spline interpolator uses the following interpolation strategies depending on the waypoint specification: + +* Linear: + + * Used, if only position is specified. + * Returns position and velocity + * Guarantees continuity at the position level. + * Discouraged because it yields trajectories with discontinuous velocities at the waypoints. + +* Cubic: + + * Used, if position and velocity are specified. + * Returns position, velocity, and acceleration. + * Guarantees continuity at the velocity level. + +* Quintic: + + * Used, if position, velocity and acceleration are specified + * Returns position, velocity, and acceleration. + * Guarantees continuity at the acceleration level. + +Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method. + +Visualized Examples +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms. + +* Sampled trajectory with linear spline if position is given only: + +.. image:: spline_position.png + :alt: Sampled trajectory with splines if position is given only + +* Sampled trajectory with cubic splines if velocity is given only (no deduction for interpolation method ``none``): + +.. image:: spline_velocity.png + :alt: Sampled trajectory with splines if velocity is given only + +* Sampled trajectory if position and velocity is given: + +.. note:: + If the same integration method was used (``Trajectory`` class uses Heun's method), then the ``spline`` method this gives identical results as above where velocity only was given as input. + +.. image:: spline_position_velocity.png + :alt: Sampled trajectory if position and velocity is given + +* Sampled trajectory with quintic splines if acceleration is given only (no deduction for interpolation method ``none``): + +.. image:: spline_acceleration.png + :alt: Sampled trajectory with splines if acceleration is given only + +* Sampled trajectory if position, velocity, and acceleration points are given: + +.. note:: + If the same integration method was used (``Trajectory`` class uses Heun's method), then the ``spline`` method this gives identical results as above where acceleration only was given as input. + +.. image:: spline_position_velocity_acceleration.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given + +* Sampled trajectory if the same position, velocity, and acceleration points as above are given, but with a nonzero initial point: + +.. image:: spline_position_velocity_acceleration_initialpoint.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given with nonzero initial point + +* Sampled trajectory if the same position, velocity, and acceleration points as above are given but with the first point starting at ``t=0``: + +.. note:: + If the first point is starting at ``t=0``, there is no interpolation from the initial point to the trajectory. + +.. image:: spline_position_velocity_acceleration_initialpoint_notime.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given with nonzero initial point and first point starting at ``t=0`` + +* Sampled trajectory with splines if inconsistent position, velocity, and acceleration points are given: + +.. note:: + Interpolation method ``none`` only gives the next input points, while the ``spline`` interpolation method shows high overshoot to match the given trajectory points. + +.. image:: spline_wrong_points.png + :alt: Sampled trajectory with splines if inconsistent position, velocity, and acceleration is given + +.. _joint_trajectory_controller_trajectory_replacement: + +Trajectory Replacement +--------------------------------- +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. [#f1]_ + +Joint trajectory messages allow to specify the time at which a new trajectory should start executing by means of the header timestamp, where zero time (the default) means "start now". + +The arrival of a new trajectory command does not necessarily mean that the controller will completely discard the currently running trajectory and substitute it with the new one. +Rather, the controller will take the useful parts of both and combine them appropriately, yielding a smarter trajectory replacement strategy. + +The steps followed by the controller for trajectory replacement are as follows: + + + Get useful parts of the new trajectory: Preserve all waypoints whose time to be reached is in the future, and discard those with times in the past. + If there are no useful parts (ie. all waypoints are in the past) the new trajectory is rejected and the current one continues execution without changes. + + + Get useful parts of the current trajectory: Preserve the current trajectory up to the start time of the new trajectory, discard the later parts. + + + Combine the useful parts of the current and new trajectories. + +The following examples describe this behavior in detail. + +The first example shows a joint which is in hold position mode (flat grey line labeled *pos hold* in the figure below). +A new trajectory (shown in red) arrives at the current time (now), which contains three waypoints and a start time in the future (*traj start*). +The time at which waypoints should be reached (``time_from_start`` member of ``trajectory_msgs/JointTrajectoryPoint``) is relative to the trajectory start time. + +The controller splices the current hold trajectory at time *traj start* and appends the three waypoints. +Notice that between now and *traj start* the previous position hold is still maintained, as the new trajectory is not supposed to start yet. +After the last waypoint is reached, its position is held until new commands arrive. + +.. image:: new_trajectory.png + :alt: Receiving a new trajectory. + +| + +The controller guarantees that the transition between the current and new trajectories will be smooth. Longer times to reach the first waypoint mean slower transitions. + +The next examples discuss the effect of sending the same trajectory to the controller with different start times. +The scenario is that of a controller executing the trajectory from the previous example (shown in red), +and receiving a new command (shown in green) with a trajectory start time set to either zero (start now), +a future time, or a time in the past. + +.. image:: trajectory_replacement_future.png + :alt: Trajectory start time in the future. + +| + +.. image:: trajectory_replacement_now.png + :alt: Zero trajectory start time (start now). + +| + +Of special interest is the last example, where the new trajectory start time and first waypoint are in the past (before now). +In this case, the first waypoint is discarded and only the second one is realized. + +.. image:: trajectory_replacement_past.png + :alt: Trajectory start time in the past. + +| + +.. [#f1] Adolfo Rodriguez: `Understanding trajectory replacement `_ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_future.png b/joint_trajectory_controller/doc/trajectory_replacement_future.png new file mode 100644 index 0000000000..582a6acce8 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_future.png differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_now.png b/joint_trajectory_controller/doc/trajectory_replacement_now.png new file mode 100644 index 0000000000..a7cd2f6bc7 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_now.png differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_past.png b/joint_trajectory_controller/doc/trajectory_replacement_past.png new file mode 100644 index 0000000000..a719f6fc66 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_past.png differ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index d225624096..9042bf6e62 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -1,56 +1,62 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/userdoc.rst + .. _joint_trajectory_controller_userdoc: joint_trajectory_controller =========================== -Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. +Controller for executing joint-space trajectories on a group of joints. +The controller interpolates in time between the points so that their distance can be arbitrary. +Even trajectories with only one point are accepted. +Trajectories are specified as a set of waypoints to be reached at specific time instants, +which the controller attempts to execute as well as the mechanism allows. +Waypoints consist of positions, and optionally velocities and accelerations. -Trajectory representation -------------------------- +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ -The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification: +Hardware interface types +------------------------------- - Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. +Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations as command interfaces: - Cubic: Position and velocity are specified. Guarantees continuity at the velocity level. +* ``position`` +* ``position``, ``velocity`` +* ``position``, ``velocity``, ``acceleration`` +* ``velocity`` +* ``effort`` - Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. +This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time: -Hardware interface type ------------------------ +* For command interfaces ``position``, the desired positions are simply forwarded to the joints, +* For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. +* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`). -The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here. +This leads to the following allowed combinations of command and state interfaces: -Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). +* With command interface ``position``, there are no restrictions for state interfaces. +* With command interface ``velocity``: -Other features --------------- + * if command interface ``velocity`` is the only one, state interfaces must include ``position, velocity`` . - Realtime-safe implementation. +* With command interface ``effort``, state interfaces must include ``position, velocity``. - Proper handling of wrapping (continuous) joints. +* With command interface ``acceleration``, state interfaces must include ``position, velocity``. - Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. +Further restrictions of state interfaces exist: -ros2_control interfaces ------------------------- +* ``velocity`` state interface cannot be used if ``position`` interface is missing. +* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present." -References -^^^^^^^^^^^ -(the controller is not yet implemented as chainable controller) +Example controller configurations can be found :ref:`below `. -States -^^^^^^^ -The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp). -Legal combinations of state interfaces are: -- ``position`` -- ``position`` and ``velocity`` -- ``position``, ``velocity`` and ``acceleration`` -- ``effort`` +Other features +-------------- -Commands -^^^^^^^^^ +* Realtime-safe implementation. + +* Proper handling of wrapping (continuous) joints. + +* Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. Using Joint Trajectory Controller(s) @@ -101,105 +107,110 @@ A yaml file for using it could be: goal: 0.03 -Details about parameters -^^^^^^^^^^^^^^^^^^^^^^^^ +Preemption policy [#f1]_ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Only one action goal can be active at any moment, or none if the topic interface is used. Path and goal tolerances are checked only for the trajectory segments of the active goal. + +When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. The trajectory is replaced in a defined way, see :ref:`trajectory replacement `. -joints (list(string)): - Joint names to control and listen to. +Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. -command_joints (list(string)): - Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. +.. _ROS 2 interface: -command_interface (list(string)): - Command interfaces provided by the hardware interface for all joints. +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - Values: [position | velocity | acceleration] (multiple allowed) +References +,,,,,,,,,,,,,,,,,, + +(the controller is not yet implemented as chainable controller) + +States +,,,,,,,,,,,,,,,,,, -state_interfaces (list(string)): - State interfaces provided by the hardware for all joints. +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. - Values: position (mandatory) [velocity, [acceleration]]. - Acceleration interface can only be used in combination with position and velocity. +Legal combinations of state interfaces are given in section `Hardware Interface Types`_. -state_publish_rate (double): - Publish-rate of the controller's "state" topic. +Commands +,,,,,,,,, - Default: 50.0 +There are two mechanisms for sending trajectories to the controller: -action_monitor_rate (double): - Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). +* via action, see :ref:`actions ` +* via topic, see :ref:`subscriber ` - Default: 20.0 +Both use the ``trajectory_msgs/msg/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. For further information on the message format, see :ref:`trajectory representation `. -allow_partial_joints_goal (boolean): - Allow joint goals defining trajectory for only some joints. +.. _Actions: -open_loop_control (boolean): - Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). +Actions [#f1]_ +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, - If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. +/follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] + Action server for commanding the controller -constraints (structure) - Default values for tolerances if no explicit values are states in JointTrajectory message. +The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. -constraints.stopped_velocity_tolerance (double) - Default value for end velocity deviation. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message `_: - Default: 0.01 +.. code-block:: markdown -constraints.goal_time (double) - Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. - Default: 0.0 (not checked) + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. -constraints..trajectory - Maximally allowed deviation from the target trajectory for a given joint. +When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). +If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. - Default: 0.0 (tolerance is not enforced) +The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances. -constraints..goal - Maximally allowed deviation from the goal (end of the trajectory) for a given joint. +.. _Subscriber: - Default: 0.0 (tolerance is not enforced) +Subscriber [#f1]_ +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +/joint_trajectory [trajectory_msgs::msg::JointTrajectory] + Topic for commanding the controller -ROS2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. +The goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. If state tolerances are violated, the trajectory is aborted and the current position is held. +Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. -~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] - Topic for commanding the controller. -~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] - Topic publishing internal states. +Publishers +,,,,,,,,,,, -~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] - Action server for commanding the controller. +/controller_state [control_msgs::msg::JointTrajectoryControllerState] + Topic publishing internal states with the update-rate of the controller manager -Specialized versions of JointTrajectoryController (TBD in ...) +Services +,,,,,,,,,,, + +/query_state [control_msgs::srv::QueryTrajectoryState] + Query controller state at any future time + + +Further information -------------------------------------------------------------- -The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_). - -The following version of the Joint Trajectory Controller are available mapping the following interfaces: - - - position_controllers::JointTrajectoryController - - Input: position, [velocity, [acceleration]] - - Output: position - - position_velocity_controllers::JointTrajectoryController - - Input: position, [velocity, [acceleration]] - - Output: position and velocity - - position_velocity_acceleration_controllers::JointTrajectoryController - - Input: position, [velocity, [acceleration]] - - Output: position, velocity and acceleration - -.. - velocity_controllers::JointTrajectoryController -.. - Input: position, [velocity, [acceleration]] -.. - Output: velocity -.. TODO(anyone): would it be possible to output velocty and acceleration? -.. (to have an vel_acc_controllers) -.. - effort_controllers::JointTrajectoryController -.. - Input: position, [velocity, [acceleration]] -.. - Output: effort - -(*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171) +.. toctree:: + :titlesonly: + + Trajectory Representation + joint_trajectory_controller Parameters + rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst> + + +.. rubric:: Footnote + +.. [#f1] Adolfo Rodriguez: `joint_trajectory_controller `_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index 766be6e0f4..f357a312c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -45,7 +45,7 @@ const std::unordered_map InterpolationMethodMa return InterpolationMethod::NONE; } else if ( - !interpolation_method.compare( + interpolation_method.compare( InterpolationMethodMap.at(InterpolationMethod::VARIABLE_DEGREE_SPLINE)) == 0) { return InterpolationMethod::VARIABLE_DEGREE_SPLINE; @@ -53,7 +53,7 @@ const std::unordered_map InterpolationMethodMa // Default else { - RCLCPP_INFO_STREAM( + RCLCPP_INFO( LOGGER, "No interpolation method parameter was given. Using the default, VARIABLE_DEGREE_SPLINE."); return InterpolationMethod::VARIABLE_DEGREE_SPLINE; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index a229489422..4fa6b25119 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -16,51 +16,41 @@ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #include +#include // for std::reference_wrapper #include -#include #include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_msgs/srv/query_trajectory_state.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_action/types.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "joint_trajectory_controller_parameters.hpp" +// auto-generated by generate_parameter_library +#include "joint_trajectory_controller/joint_trajectory_controller_parameters.hpp" using namespace std::chrono_literals; // NOLINT -namespace rclcpp_action -{ -template -class ServerGoalHandle; -} // namespace rclcpp_action -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle - namespace joint_trajectory_controller { -class Trajectory; class JointTrajectoryController : public controller_interface::ControllerInterface { @@ -69,15 +59,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JointTrajectoryController(); /** - * @brief command_interface_configuration This controller requires the position command - * interfaces for the controlled joints + * @brief command_interface_configuration */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** - * @brief command_interface_configuration This controller requires the position and velocity - * state interfaces for the controlled joints + * @brief command_interface_configuration */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; @@ -109,10 +97,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - protected: // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants @@ -125,6 +109,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_current_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -166,24 +151,34 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; + // Configuration for every joint if it wraps around (ie. is continuous, position error is + // normalized) + std::vector joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; + // Timeout to consider commands old + double cmd_timeout_; + // True if holding position or repeating last trajectory point in case of success + realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; - std::shared_ptr * traj_point_active_ptr_ = nullptr; + rclcpp::Service::SharedPtr query_state_srv_; + std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; + std::shared_ptr hold_position_msg_ptr_ = nullptr; + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr publisher_legacy_; + StatePublisherPtr state_publisher_legacy_; rclcpp::Publisher::SharedPtr publisher_; StatePublisherPtr state_publisher_; @@ -197,6 +192,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp_action::Server::SharedPtr action_server_; RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + realtime_tools::RealtimeBuffer rt_has_pending_goal_; ///< Is there a pending action goal? rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); @@ -234,32 +230,64 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + // the tolerances from the node parameter SegmentTolerances default_tolerances_; + // the tolerances used for the current goal + realtime_tools::RealtimeBuffer active_tolerances_; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_hold_position(); + std::shared_ptr set_hold_position(); + + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr set_success_trajectory_point(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_active_trajectory() const; + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_hardware(JointTrajectoryPoint & state); + void read_state_from_state_interfaces(JointTrajectoryPoint & state); + /** Assign values from the command interfaces as state. + * This is only possible if command AND state interfaces exist for the same type, + * therefore needs check for both. + * @param[out] state to be filled with values from command interfaces. + * @return true if all interfaces exists and contain non-NaN values, false otherwise. + */ bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); + + void query_state_service( + const std::shared_ptr request, + std::shared_ptr response); private: + void update_pids(); + bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); + void init_hold_position_msg(); void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + void resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index de8f060b1a..4cee2ae883 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,15 +30,16 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ -#include -#include +#include +#include #include #include #include "control_msgs/action/follow_joint_trajectory.hpp" -#include "joint_trajectory_controller_parameters.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" namespace joint_trajectory_controller { @@ -88,16 +89,19 @@ struct SegmentTolerances * goal: 0.01 * \endcode * + * \param jtc_logger The logger to use for output * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -109,25 +113,190 @@ SegmentTolerances get_segment_tolerances(Params const & params) tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory.position").c_str(), + tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (joint + ".goal.position").c_str(), + tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal.velocity").c_str(), + tolerances.goal_state_tolerance[i].velocity); } return tolerances; } +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [=](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + if (goal_value > 0.0) + { + return goal_value; + } + else if (is_erase_value(goal_value)) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value."); + } + return default_value; +} + +/** + * \brief Populate trajectory segment tolerances using data from an action goal. + * + * \param jtc_logger The logger to use for output + * \param default_tolerances The default tolerances to use if the action goal does not specify any. + * \param goal The new action goal + * \param joints The joints configured by ROS parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances( + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) +{ + SegmentTolerances active_tolerances(default_tolerances); + static auto logger = jtc_logger.get_child("tolerance"); + + try + { + active_tolerances.goal_time_tolerance = resolve_tolerance_source( + default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR( + logger, "Specified illegal goal_time_tolerance: %f. Using default tolerances", + rclcpp::Duration(goal.goal_time_tolerance).seconds()); + return default_tolerances; + } + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + + // State and goal state tolerances + for (auto joint_tol : goal.path_tolerance) + { + auto const joint = joint_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = static_cast(std::distance(joints.cbegin(), it)); + std::string interface = ""; + try + { + interface = "position"; + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR( + logger, + "joint '%s' specified in goal.path_tolerance has a invalid %s tolerance. Using default " + "tolerances.", + joint.c_str(), interface.c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.position").c_str(), + active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), + active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), + active_tolerances.state_tolerance[i].acceleration); + } + for (auto goal_tol : goal.goal_tolerance) + { + auto const joint = goal_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = static_cast(std::distance(joints.cbegin(), it)); + std::string interface = ""; + try + { + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR( + logger, + "joint '%s' specified in goal.goal_tolerance has a invalid %s tolerance. Using default " + "tolerances.", + joint.c_str(), interface.c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), + active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), + active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), + active_tolerances.goal_state_tolerance[i].acceleration); + } + + return active_tolerances; +} + /** * \param state_error State error to check. * \param joint_idx Joint index for the state error * \param state_tolerance State tolerance of joint to check \p state_error against. - * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true - * \return True if \p state_error fulfills \p state_tolerance. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT + * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. */ inline bool check_state_tolerance_per_joint( - const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, const StateTolerances & state_tolerance, bool show_errors = false) { using std::abs; @@ -150,7 +319,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); + RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 7dac4ddbe1..d47e3f37a1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -44,14 +44,19 @@ class Trajectory const trajectory_msgs::msg::JointTrajectoryPoint & current_point, std::shared_ptr joint_trajectory); - /// Set the point before the trajectory message is replaced/appended - /// Example: if we receive a new trajectory message and it's first point is 0.5 seconds - /// from the current one, we call this function to log the current state, then - /// append/replace the current trajectory + /** + * Set the point before the trajectory message is replaced/appended + * Example: if we receive a new trajectory message and it's first point is 0.5 seconds + * from the current one, we call this function to log the current state, then + * append/replace the current trajectory + * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that + * wrap around (ie. is continuous). + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC void set_point_before_trajectory_msg( const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point); + const trajectory_msgs::msg::JointTrajectoryPoint & current_point, + const std::vector & joints_angle_wraparound = std::vector()); JOINT_TRAJECTORY_CONTROLLER_PUBLIC void update(std::shared_ptr joint_trajectory); @@ -60,26 +65,35 @@ class Trajectory /// containing trajectory. /** * Sampling trajectory at given \p sample_time. - * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively. - * Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment. + * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or + * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to + * be reached at the time defined in the segment. + * + * This function assumes that sampling is only done at monotonically increasing \p sample_time + * for any trajectory. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() * - Sampling exactly on a point of the trajectory: - * start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr + * start_segment_itr = iterator where point is, end_segment_itr = iterator after + * start_segment_itr * - Sampling between points: - * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr + * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after + * start_segment_itr * - Sampling after entire trajectory: * start_segment_itr = --end(), end_segment_itr = end() * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() * return false * * \param[in] sample_time Time at which trajectory will be sampled. - * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at all. - * \param[out] expected_state Calculated new at \p sample_time. - * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above. - * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at + * all. + * \param[out] output_state Calculated new at \p sample_time. + * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See + * description above. + * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See + * description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -91,14 +105,16 @@ class Trajectory /** * Do interpolation between 2 states given a time in between their respective timestamps * - * The start and end states need not necessarily be specified all the way to the acceleration level: + * The start and end states need not necessarily be specified all the way to the acceleration + * level: * - If only \b positions are specified, linear interpolation will be used. * - If \b positions and \b velocities are specified, a cubic spline will be used. - * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used. + * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be + * used. * * If start and end states have different specifications - * (eg. start is position-only, end is position-velocity), the lowest common specification will be used - * (position-only in the example). + * (eg. start is position-only, end is position-velocity), the lowest common specification will be + * used (position-only in the example). * * \param[in] time_a Time at which the segment state equals \p state_a. * \param[in] state_a State at \p time_a. @@ -125,6 +141,9 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_trajectory_msg() const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_nontrivial_msg() const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr get_trajectory_msg() const { @@ -132,10 +151,15 @@ class Trajectory } JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } + bool is_sampled_already() const { return sampled_already_; } + /// Get the index of the segment start returned by the last \p sample() operation. + /** + * As the trajectory is only accessed at monotonically increasing sampling times, this index is + * used to speed up the selection of relevant trajectory points. + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool is_sampled_already() const { return sampled_already_; } + size_t last_sample_index() const { return last_sample_idx_; } private: void deduce_from_derivatives( @@ -150,12 +174,13 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + size_t last_sample_idx_ = 0; }; /** - * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 indices. - * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is - * "{2, 1}". + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated + * mapping vector is "{2, 1}". */ template inline std::vector mapping(const T & t1, const T & t2) @@ -176,14 +201,25 @@ inline std::vector mapping(const T & t1, const T & t2) } else { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); + const size_t t1_dist = static_cast(std::distance(t1.begin(), t1_it)); + const size_t t2_dist = static_cast(std::distance(t2.begin(), t2_it)); mapping_vector[t1_dist] = t2_dist; } } return mapping_vector; } +/** + * \param current_position The current position given from the controller, which will be adapted. + * \param next_position Next position from which to compute the wraparound offset, i.e., + * the first trajectory point + * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that + * wrap around (ie. is continuous). + */ +void wraparound_joint( + std::vector & current_position, const std::vector next_position, + const std::vector & joints_angle_wraparound); + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index 09363eebb1..0b22c7cdd9 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -16,12 +16,17 @@ #define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ #include +#include #include "parameter_traits/parameter_traits.hpp" +#include "rclcpp/parameter.hpp" +#include "rsl/algorithm.hpp" +#include "tl_expected/expected.hpp" -namespace parameter_traits +namespace joint_trajectory_controller { -Result command_interface_type_combinations(rclcpp::Parameter const & parameter) +tl::expected command_interface_type_combinations( + rclcpp::Parameter const & parameter) { auto const & interface_types = parameter.as_string_array(); @@ -31,33 +36,37 @@ Result command_interface_type_combinations(rclcpp::Parameter const & parameter) // 2. position [velocity, [acceleration]] if ( - contains(interface_types, "velocity") && interface_types.size() > 1 && - !contains(interface_types, "position")) + rsl::contains>(interface_types, "velocity") && + interface_types.size() > 1 && + !rsl::contains>(interface_types, "position")) { - return ERROR( + return tl::make_unexpected( "'velocity' command interface can be used either alone or 'position' " - "interface has to be present"); + "command interface has to be present"); } if ( - contains(interface_types, "acceleration") && - (!contains(interface_types, "velocity") && - !contains(interface_types, "position"))) + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position"))) { - return ERROR( + return tl::make_unexpected( "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); + "'position' command interfaces are present"); } - if (contains(interface_types, "effort") && interface_types.size() > 1) + if ( + rsl::contains>(interface_types, "effort") && + interface_types.size() > 1) { - return ERROR("'effort' command interface has to be used alone"); + return tl::make_unexpected("'effort' command interface has to be used alone"); } - return OK; + return {}; } -Result state_interface_type_combinations(rclcpp::Parameter const & parameter) +tl::expected state_interface_type_combinations( + rclcpp::Parameter const & parameter) { auto const & interface_types = parameter.as_string_array(); @@ -65,27 +74,27 @@ Result state_interface_type_combinations(rclcpp::Parameter const & parameter) // 1. position [velocity, [acceleration]] if ( - contains(interface_types, "velocity") && - !contains(interface_types, "position")) + rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position")) { - return ERROR( + return tl::make_unexpected( "'velocity' state interface cannot be used if 'position' interface " "is missing."); } if ( - contains(interface_types, "acceleration") && - (!contains(interface_types, "position") || - !contains(interface_types, "velocity"))) + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "position") || + !rsl::contains>(interface_types, "velocity"))) { - return ERROR( + return tl::make_unexpected( "'acceleration' state interface cannot be used if 'position' and 'velocity' " "interfaces are not present."); } - return OK; + return {}; } -} // namespace parameter_traits +} // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3e806ce59e..8c11462827 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,35 +1,40 @@ joint_trajectory_controller - 2.15.0 + 2.44.0 Controller for executing joint-space trajectories on a group of joints + Bence Magyar - Jordan Palacios - Karsten Knese + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 - ament_cmake - - angles - pluginlib - realtime_tools + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ - angles - pluginlib - realtime_tools + ament_cmake + angles + backward_ros controller_interface control_msgs control_toolbox + generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle + realtime_tools + rsl + tl_expected trajectory_msgs - generate_parameter_library ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 96110adcb2..e72d442441 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -14,12 +14,10 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include #include #include #include -#include -#include + #include #include @@ -32,16 +30,12 @@ #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { @@ -67,6 +61,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() return CallbackReturn::ERROR; } + // TODO(christophfroehlich): remove deprecation warning + if (params_.allow_nonzero_velocity_at_trajectory_end) + { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to " + "true. The default behavior will change to false."); + } + return CallbackReturn::SUCCESS; } @@ -118,16 +121,40 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); + // update dynamic parameters + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + default_tolerances_ = get_segment_tolerances(logger, params_); + // update the PID gains + // variable use_closed_loop_pid_adapter_ is updated in on_configure only + if (use_closed_loop_pid_adapter_) + { + update_pids(); + } + } auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, + JointTrajectoryPoint & error, size_t index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); - if (has_velocity_state_interface_ && has_velocity_command_interface_) + if (joints_angle_wraparound_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -piget_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) + // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + if ( + current_external_msg != *new_external_msg && + (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -161,39 +194,61 @@ controller_interface::return_type JointTrajectoryController::update( // current state update state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); + read_state_from_state_interfaces(state_current_); // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + if (has_active_trajectory()) { bool first_sample = false; // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) + if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; if (params_.open_loop_control) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + traj_external_point_ptr_->set_point_before_trajectory_msg( + time, last_commanded_state_, joints_angle_wraparound_); } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); + traj_external_point_ptr_->set_point_before_trajectory_msg( + time, state_current_, joints_angle_wraparound_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = - (*traj_point_active_ptr_) - ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + const bool valid_point = traj_external_point_ptr_->sample( + time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); if (valid_point) { + const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + // this is the time instance + // - started with the first segment: when the first point will be reached (in the future) + // - later: when the point of the current segment was reached + const rclcpp::Time segment_time_from_start = traj_start + start_segment_itr->time_from_start; + // time_difference is + // - negative until first point is reached + // - counting from zero to time_from_start of next point + double time_difference = time.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - double time_difference = 0.0; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); + const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + auto active_tol = active_tolerances_.readFromRT(); + + // have we reached the end, are not holding position, and is a timeout configured? + // Check independently of other tolerances + if ( + !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && + time_difference > cmd_timeout_) + { + RCLCPP_WARN(logger, "Aborted due to command timeout"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) @@ -202,32 +257,32 @@ controller_interface::return_type JointTrajectoryController::update( // Always check the state tolerance on the first sample in case the first sample // is the last point + // print output per default, goal will be aborted afterwards if ( - (before_last_point || first_sample) && + (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && + !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) + if (active_tol->goal_time_tolerance != 0.0) { // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - time_difference = get_node()->now().seconds() - traj_end.seconds(); - - if (time_difference > default_tolerances_.goal_time_tolerance) + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); } } } @@ -243,8 +298,7 @@ controller_interface::return_type JointTrajectoryController::update( { tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( - state_desired_.positions[i] - state_current_.positions[i], - state_desired_.velocities[i] - state_current_.velocities[i], + state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); } } @@ -271,21 +325,13 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_effort_command_interface_) { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } + assign_interface_from_point(joint_command_interface_[3], tmp_command_); } // store the previous command. Used in open-loop control mode last_commanded_state_ = state_desired_; } - const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { // send feedback @@ -301,53 +347,79 @@ controller_interface::return_type JointTrajectoryController::update( // check abort if (tolerance_violated_while_moving) { - set_hold_position(); auto result = std::make_shared(); - - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); + + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); - // check goal tolerance + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } + // check goal tolerance else if (!before_last_point) { if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { - set_hold_position(); + const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " + + std::to_string(time_difference) + " seconds"; + auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + rt_has_pending_goal_.writeFromNonRT(false); + + RCLCPP_WARN(logger, "%s", error_string.c_str()); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } - // else, run another cycle while waiting for outside_goal_tolerance - // to be satisfied or violated within the goal_time_tolerance } } - else if (tolerance_violated_while_moving) + else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) + { + // we need to ensure that there is no pending goal -> we get a race condition otherwise + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + else if ( + !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { - set_hold_position(); - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied (will stay in this state until new message arrives) + // or outside_goal_tolerance violated within the goal_time_tolerance } } @@ -355,7 +427,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) @@ -408,8 +480,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -462,14 +533,134 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } +bool JointTrajectoryController::read_commands_from_command_interfaces( + JointTrajectoryPoint & commands) +{ + bool has_values = true; + + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + }; + + // Assign values from the command interfaces as command. + if (has_position_command_interface_) + { + if (interface_has_values(joint_command_interface_[0])) + { + assign_point_from_interface(commands.positions, joint_command_interface_[0]); + } + else + { + commands.positions.clear(); + has_values = false; + } + } + if (has_velocity_command_interface_) + { + if (interface_has_values(joint_command_interface_[1])) + { + assign_point_from_interface(commands.velocities, joint_command_interface_[1]); + } + else + { + commands.velocities.clear(); + has_values = false; + } + } + if (has_acceleration_command_interface_) + { + if (interface_has_values(joint_command_interface_[2])) + { + assign_point_from_interface(commands.accelerations, joint_command_interface_[2]); + } + else + { + commands.accelerations.clear(); + has_values = false; + } + } + if (has_effort_command_interface_) + { + if (interface_has_values(joint_command_interface_[3])) + { + assign_point_from_interface(commands.effort, joint_command_interface_[3]); + } + else + { + commands.effort.clear(); + has_values = false; + } + } + + return has_values; +} + +void JointTrajectoryController::query_state_service( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto logger = get_node()->get_logger(); + // Preconditions + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); + response->success = false; + return; + } + const auto active_goal = *rt_active_goal_.readFromRT(); + response->name = params_.joints; + trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; + if (has_active_trajectory()) + { + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + response->success = traj_external_point_ptr_->sample( + static_cast(request->time), interpolation_method_, state_requested, + start_segment_itr, end_segment_itr); + // If the requested sample time precedes the trajectory finish time respond as failure + if (response->success) + { + if (end_segment_itr == traj_external_point_ptr_->end()) + { + RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); + response->success = false; + } + } + else + { + RCLCPP_ERROR( + logger, "Requested sample time is earlier than the current trajectory start time."); + } + } + else + { + RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance."); + response->success = false; + } + response->position = state_requested.positions; + response->velocity = state_requested.velocities; + response->acceleration = state_requested.accelerations; +} + controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = get_node()->get_logger(); + auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } @@ -479,17 +670,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); - // Check if the DoF has changed - if ((dof_ > 0) && (params_.joints.size() != dof_)) - { - RCLCPP_ERROR( - logger, - "The JointTrajectoryController does not support restarting with a different number of DOF"); - // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we - // can continue - return CallbackReturn::FAILURE; - } - + // get degrees of freedom dof_ = params_.joints.size(); // TODO(destogl): why is this here? Add comment or move @@ -519,12 +700,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - // // Specialized, child controllers set interfaces before calling configure function. - // if (command_interface_types_.empty()) - // { - // command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); - // } - if (params_.command_interfaces.empty()) { RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); @@ -547,7 +722,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // if there is only velocity or if there is effort command interface // then use also PID adapter use_closed_loop_pid_adapter_ = - (has_velocity_command_interface_ && params_.command_interfaces.size() == 1) || + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + !params_.open_loop_control) || has_effort_command_interface_; if (use_closed_loop_pid_adapter_) @@ -556,28 +732,23 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - - // TODO(destogl): remove this in ROS2 Iron - // Check deprecated style for "ff_velocity_scale" parameter definition. - if (gains.ff_velocity_scale == 0.0) - { - RCLCPP_WARN( - get_node()->get_logger(), - "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " - "Maybe you are using deprecated format 'ff_velocity_scale/'!"); + update_pids(); + } - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); - } - else - { - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } + // Configure joint position error normalization from ROS parameters (angle_wraparound) + joints_angle_wraparound_.resize(dof_); + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + if (gains.normalize_error) + { + // TODO(anyone): Remove deprecation warning in the end of 2023 + RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!"); + joints_angle_wraparound_[i] = gains.normalize_error; + } + else + { + joints_angle_wraparound_[i] = gains.angle_wraparound; } } @@ -645,8 +816,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.command_interfaces).c_str(), get_interface_list(params_.state_interfaces).c_str()); - default_tolerances_ = get_segment_tolerances(params_); - + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(logger, params_); + active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -654,6 +826,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( logger, "Using '%s' interpolation method.", interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + // prepare hold_position_msg + init_hold_position_msg(); + + // create subscriber and publishers joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), @@ -670,27 +846,66 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = + publisher_legacy_ = get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_legacy_ = std::make_unique(publisher_legacy_); + + state_publisher_legacy_->lock(); + state_publisher_legacy_->msg_.joint_names = params_.joints; + state_publisher_legacy_->msg_.desired.positions.resize(dof_); + state_publisher_legacy_->msg_.desired.velocities.resize(dof_); + state_publisher_legacy_->msg_.desired.accelerations.resize(dof_); + state_publisher_legacy_->msg_.actual.positions.resize(dof_); + state_publisher_legacy_->msg_.error.positions.resize(dof_); + if (has_velocity_state_interface_) + { + state_publisher_legacy_->msg_.actual.velocities.resize(dof_); + state_publisher_legacy_->msg_.error.velocities.resize(dof_); + } + if (has_acceleration_state_interface_) + { + state_publisher_legacy_->msg_.actual.accelerations.resize(dof_); + state_publisher_legacy_->msg_.error.accelerations.resize(dof_); + } + state_publisher_legacy_->unlock(); + + publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); state_publisher_->msg_.joint_names = params_.joints; - state_publisher_->msg_.desired.positions.resize(dof_); - state_publisher_->msg_.desired.velocities.resize(dof_); - state_publisher_->msg_.desired.accelerations.resize(dof_); - state_publisher_->msg_.actual.positions.resize(dof_); + state_publisher_->msg_.reference.positions.resize(dof_); + state_publisher_->msg_.reference.velocities.resize(dof_); + state_publisher_->msg_.reference.accelerations.resize(dof_); + state_publisher_->msg_.feedback.positions.resize(dof_); state_publisher_->msg_.error.positions.resize(dof_); if (has_velocity_state_interface_) { - state_publisher_->msg_.actual.velocities.resize(dof_); + state_publisher_->msg_.feedback.velocities.resize(dof_); state_publisher_->msg_.error.velocities.resize(dof_); } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.feedback.accelerations.resize(dof_); state_publisher_->msg_.error.accelerations.resize(dof_); } + if (has_position_command_interface_) + { + state_publisher_->msg_.output.positions.resize(dof_); + } + if (has_velocity_command_interface_) + { + state_publisher_->msg_.output.velocities.resize(dof_); + } + if (has_acceleration_command_interface_) + { + state_publisher_->msg_.output.accelerations.resize(dof_); + } + if (has_effort_command_interface_) + { + state_publisher_->msg_.output.effort.resize(dof_); + } state_publisher_->unlock(); last_state_publish_time_ = get_node()->now(); @@ -715,28 +930,44 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point_command(command_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); + query_state_srv_ = get_node()->create_service( + std::string(get_node()->get_name()) + "/query_state", + std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + auto logger = get_node()->get_logger(); + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(logger, params_); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); + logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(), + joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -744,54 +975,66 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); + logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), + joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); traj_msg_external_point_ptr_.writeFromNonRT( std::shared_ptr()); subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; last_state_publish_time_ = get_node()->now(); - // Initialize current state storage if hardware state has tracking offset - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading from commands if - // those are not nan + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if ( + params_.set_last_command_interface_value_as_state_on_activation && + read_state_from_command_interfaces(state)) { state_current_ = state; - state_desired_ = state; last_commanded_state_ = state; } + else + { + // Initialize current state storage from hardware + read_state_from_state_interfaces(state_current_); + read_state_from_state_interfaces(last_commanded_state_); + } + + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); + rt_is_holding_.writeFromNonRT(true); + + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } return CallbackReturn::SUCCESS; } @@ -799,7 +1042,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { - // TODO(anyone): How to halt when using effort commands? + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + rt_has_pending_goal_.writeFromNonRT(false); + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled during deactivate transition."); + active_goal->setAborted(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + for (size_t index = 0; index < dof_; ++index) { if (has_position_command_interface_) @@ -813,6 +1066,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( joint_command_interface_[1][index].get().set_value(0.0); } + if (has_acceleration_command_interface_) + { + joint_command_interface_[2][index].get().set_value(0.0); + } + + // TODO(anyone): How to halt when using effort commands? if (has_effort_command_interface_) { joint_command_interface_[3][index].get().set_value(0.0); @@ -828,16 +1087,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; + traj_external_point_ptr_.reset(); + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { - // go home - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; - return CallbackReturn::SUCCESS; } @@ -864,24 +1121,11 @@ bool JointTrajectoryController::reset() } } - // iterator has no default value - // prev_traj_point_ptr_; - traj_point_active_ptr_ = nullptr; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return true; } -controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - // TODO(karsten1987): what to do? - - return CallbackReturn::SUCCESS; -} - void JointTrajectoryController::publish_state( const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) @@ -896,25 +1140,65 @@ void JointTrajectoryController::publish_state( return; } + if (state_publisher_legacy_ && state_publisher_legacy_->trylock()) + { + last_state_publish_time_ = get_node()->now(); + state_publisher_legacy_->msg_.header.stamp = last_state_publish_time_; + state_publisher_legacy_->msg_.desired.positions = desired_state.positions; + state_publisher_legacy_->msg_.desired.velocities = desired_state.velocities; + state_publisher_legacy_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_legacy_->msg_.actual.positions = current_state.positions; + state_publisher_legacy_->msg_.error.positions = state_error.positions; + if (has_velocity_state_interface_) + { + state_publisher_legacy_->msg_.actual.velocities = current_state.velocities; + state_publisher_legacy_->msg_.error.velocities = state_error.velocities; + } + if (has_acceleration_state_interface_) + { + state_publisher_legacy_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_legacy_->msg_.error.accelerations = state_error.accelerations; + } + + state_publisher_legacy_->unlockAndPublish(); + + if (publisher_legacy_->get_subscription_count()) + { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Subscription to deprecated ~/state topic. Use ~/controller_state instead."); + } + } + if (state_publisher_ && state_publisher_->trylock()) { last_state_publish_time_ = get_node()->now(); state_publisher_->msg_.header.stamp = last_state_publish_time_; - state_publisher_->msg_.desired.positions = desired_state.positions; - state_publisher_->msg_.desired.velocities = desired_state.velocities; - state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - state_publisher_->msg_.actual.positions = current_state.positions; + state_publisher_->msg_.reference.positions = desired_state.positions; + state_publisher_->msg_.reference.velocities = desired_state.velocities; + state_publisher_->msg_.reference.accelerations = desired_state.accelerations; + state_publisher_->msg_.feedback.positions = current_state.positions; + // DESIRED and ACTUAL are deprecated in the message but we are still + // reporting on them + state_publisher_legacy_->msg_.desired.positions = desired_state.positions; + state_publisher_legacy_->msg_.desired.velocities = desired_state.velocities; + state_publisher_legacy_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_legacy_->msg_.actual.positions = current_state.positions; state_publisher_->msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) { - state_publisher_->msg_.actual.velocities = current_state.velocities; + state_publisher_->msg_.feedback.velocities = current_state.velocities; state_publisher_->msg_.error.velocities = state_error.velocities; } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_->msg_.feedback.accelerations = current_state.accelerations; state_publisher_->msg_.error.accelerations = state_error.accelerations; } + if (read_commands_from_command_interfaces(command_current_)) + { + state_publisher_->msg_.output = command_current_; + } state_publisher_->unlockAndPublish(); } @@ -932,6 +1216,7 @@ void JointTrajectoryController::topic_callback( if (subscriber_is_active_) { add_new_trajectory_msg(msg); + rt_is_holding_.writeFromNonRT(false); } }; @@ -966,17 +1251,17 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal && active_goal->gh_ == goal_handle) { - // Controller uptime - // Enter hold current position mode - set_hold_position(); - - RCLCPP_DEBUG( + RCLCPP_INFO( get_node()->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled + rt_has_pending_goal_.writeFromNonRT(false); auto action_res = std::make_shared(); active_goal->setCanceled(action_res); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + + // Enter hold current position mode + add_new_trajectory_msg(set_hold_position()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -984,6 +1269,9 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback void JointTrajectoryController::goal_accepted_callback( std::shared_ptr> goal_handle) { + // mark a pending goal + rt_has_pending_goal_.writeFromNonRT(true); + // Update new trajectory { preempt_active_goal(); @@ -991,6 +1279,7 @@ void JointTrajectoryController::goal_accepted_callback( std::make_shared(goal_handle->get_goal()->trajectory); add_new_trajectory_msg(traj_msg); + rt_is_holding_.writeFromNonRT(false); } // Update the active goal @@ -999,6 +1288,11 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Update tolerances if specified in the goal + auto logger = this->get_node()->get_logger(); + active_tolerances_.writeFromNonRT(get_segment_tolerances( + logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -1087,8 +1381,12 @@ void JointTrajectoryController::sort_to_local_joint_order( get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } - std::vector output; - output.resize(mapping.size(), 0.0); + static std::vector output(dof_, 0.0); + // Only resize if necessary since it's an expensive operation + if (output.size() != mapping.size()) + { + output.resize(mapping.size(), 0.0); + } for (size_t index = 0; index < mapping.size(); ++index) { auto map_index = mapping[index]; @@ -1124,8 +1422,9 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + get_node()->get_logger(), + "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size, + string_for_vector_field.c_str(), vector_field.size(), i); return false; } return true; @@ -1152,22 +1451,25 @@ bool JointTrajectoryController::validate_trajectory_msg( return false; } + if (trajectory.points.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; + } + const auto trajectory_start_time = static_cast(trajectory.header.stamp); // If the starting time it set to 0.0, it means the controller should start it now. // Otherwise we check if the trajectory ends before the current time, // in which case it can be ignored. if (trajectory_start_time.seconds() != 0.0) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } + auto const trajectory_end_time = + trajectory_start_time + trajectory.points.back().time_from_start; if (trajectory_end_time < get_node()->now()) { RCLCPP_ERROR( get_node()->get_logger(), - "Received trajectory with non zero time start time (%f) that ends on the past (%f)", + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; } @@ -1187,6 +1489,21 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + if (!params_.allow_nonzero_velocity_at_trajectory_end) + { + for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) + { + if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits::epsilon()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Velocity of last trajectory point of joint %s is not zero: %.15f", + trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); + return false; + } + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { @@ -1206,8 +1523,15 @@ bool JointTrajectoryController::validate_trajectory_msg( // This currently supports only position, velocity and acceleration inputs if (params_.allow_integration_in_goal_trajectories) { - const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && - points[i].accelerations.empty(); + if ( + points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The given trajectory has no position, velocity, or acceleration points."); + return false; + } const bool position_error = !points[i].positions.empty() && !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); @@ -1218,7 +1542,7 @@ bool JointTrajectoryController::validate_trajectory_msg( !points[i].accelerations.empty() && !validate_trajectory_point_field( joint_count, points[i].accelerations, "accelerations", i, false); - if (all_empty || position_error || velocity_error || acceleration_error) + if (position_error || velocity_error || acceleration_error) { return false; } @@ -1227,9 +1551,15 @@ bool JointTrajectoryController::validate_trajectory_msg( !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true) || - !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) + joint_count, points[i].accelerations, "accelerations", i, true)) + { + return false; + } + // reject effort entries + if (!points[i].effort.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), "Trajectories with effort fields are currently not supported."); return false; } } @@ -1247,7 +1577,6 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { - set_hold_position(); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); @@ -1256,13 +1585,30 @@ void JointTrajectoryController::preempt_active_goal() } } -void JointTrajectoryController::set_hold_position() +std::shared_ptr +JointTrajectoryController::set_hold_position() { - trajectory_msgs::msg::JointTrajectory empty_msg; - empty_msg.header.stamp = rclcpp::Time(0); + // Command to stay at current position + hold_position_msg_ptr_->points[0].positions = state_current_.positions; + + // set flag, otherwise tolerances will be checked with holding position too + rt_is_holding_.writeFromNonRT(true); - auto traj_msg = std::make_shared(empty_msg); - add_new_trajectory_msg(traj_msg); + return hold_position_msg_ptr_; +} + +std::shared_ptr +JointTrajectoryController::set_success_trajectory_point() +{ + // set last command to be repeated at success, no matter if it has nonzero velocity or + // acceleration + hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); + + // set flag, otherwise tolerances will be checked with success_trajectory_point too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; } bool JointTrajectoryController::contains_interface_type( @@ -1286,6 +1632,87 @@ void JointTrajectoryController::resize_joint_trajectory_point( } } +void JointTrajectoryController::resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + if (has_position_command_interface_) + { + point.positions.resize(size, 0.0); + } + if (has_velocity_command_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_command_interface_) + { + point.accelerations.resize(size, 0.0); + } + if (has_effort_command_interface_) + { + point.effort.resize(size, 0.0); + } +} + +bool JointTrajectoryController::has_active_trajectory() const +{ + return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); +} + +void JointTrajectoryController::update_pids() +{ + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + if (pids_[i]) + { + // update PIDs with gains from ROS parameters + pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + else + { + // Init PIDs with gains from ROS parameters + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + // Check deprecated style for "ff_velocity_scale" parameter definition. + if (gains.ff_velocity_scale == 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " + "Maybe you are using deprecated format 'ff_velocity_scale/'!"); + + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + } + else + { + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + } +} + +void JointTrajectoryController::init_hold_position_msg() +{ + hold_position_msg_ptr_ = std::make_shared(); + hold_position_msg_ptr_->header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + hold_position_msg_ptr_->joint_names = params_.joints; + hold_position_msg_ptr_->points.resize(1); // a trivial msg only + hold_position_msg_ptr_->points[0].velocities.clear(); + hold_position_msg_ptr_->points[0].accelerations.clear(); + hold_position_msg_ptr_->points[0].effort.clear(); + if (has_velocity_command_interface_ || has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns velocity points in any case + hold_position_msg_ptr_->points[0].velocities.resize(dof_, 0.0); + } + if (has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns acceleration points in any case + hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 80aa32585b..ec54363188 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -2,7 +2,8 @@ joint_trajectory_controller: joints: { type: string_array, default_value: [], - description: "Names of joints used by the controller", + description: "Joint names to control and listen to", + read_only: true, validation: { unique<>: null, } @@ -10,7 +11,8 @@ joint_trajectory_controller: command_joints: { type: string_array, default_value: [], - description: "Names of the commanding joints used by the controller", + description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.", + read_only: true, validation: { unique<>: null, } @@ -18,32 +20,44 @@ joint_trajectory_controller: command_interfaces: { type: string_array, default_value: [], - description: "Names of command interfaces to claim", + description: "Command interfaces provided by the hardware interface for all joints", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], - command_interface_type_combinations: null, + "joint_trajectory_controller::command_interface_type_combinations": null, } } state_interfaces: { type: string_array, default_value: [], - description: "Names of state interfaces to claim", + description: "State interfaces provided by the hardware for all joints.", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], - state_interface_type_combinations: null, + "joint_trajectory_controller::state_interface_type_combinations": null, } } allow_partial_joints_goal: { type: bool, default_value: false, - description: "Goals with partial set of joints are allowed", + description: "Allow joint goals defining trajectory for only some joints.", } open_loop_control: { type: bool, default_value: false, - description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + description: "Use controller in open-loop control mode + \n\n + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n + * It deactivates the feedback control, see the ``gains`` structure. + \n\n + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + \n\n + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started.\n", + read_only: true, } allow_integration_in_goal_trajectories: { type: bool, @@ -55,51 +69,85 @@ joint_trajectory_controller: default_value: 50.0, description: "Rate controller state is published", validation: { - lower_bounds: [0.1] + gt_eq: [0.1] } } + set_last_command_interface_value_as_state_on_activation: { + type: bool, + default_value: true, + description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.", + } action_monitor_rate: { type: double, default_value: 20.0, - description: "Rate status changes will be monitored", + description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)", + read_only: true, validation: { - lower_bounds: [0.1] + gt_eq: [0.1] } } interpolation_method: { type: string, default_value: "splines", description: "The type of interpolation to use, if any", + read_only: true, validation: { one_of<>: [["splines", "none"]], } } + allow_nonzero_velocity_at_trajectory_end: { + type: bool, + default_value: true, + description: "If false, the last velocity point has to be zero or the goal will be rejected", + } + cmd_timeout: { + type: double, + default_value: 0.0, # seconds + description: "Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. + If zero, timeout is deactivated", + } gains: __map_joints: p: { type: double, default_value: 0.0, - description: "Proportional gain for PID" + description: "Proportional gain :math:`k_p` for PID" } i: { type: double, default_value: 0.0, - description: "Intigral gain for PID" + description: "Integral gain :math:`k_i` for PID" } d: { type: double, default_value: 0.0, - description: "Derivative gain for PID" + description: "Derivative gain :math:`k_d` for PID" } i_clamp: { type: double, default_value: 0.0, - description: "Integrale clamp. Symmetrical in both positive and negative direction." + description: "Integral clamp. Symmetrical in both positive and negative direction." } ff_velocity_scale: { type: double, default_value: 0.0, - description: "Feed-forward scaling of velocity." + description: "Feed-forward scaling :math:`k_{ff}` of velocity" + } + normalize_error: { + type: bool, + default_value: false, + description: "(Deprecated) Use position error normalization to -pi to pi." + } + angle_wraparound: { + type: bool, + default_value: false, + description: 'For joints that wrap around (without end stop, ie. are continuous), + where the shortest rotation to the target position is the desired motion. + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface.' } constraints: stopped_velocity_tolerance: { @@ -110,9 +158,10 @@ joint_trajectory_controller: goal_time: { type: double, default_value: 0.0, - description: "Time tolerance for achieving trajectory goal before or after commanded time.", + description: "Time tolerance for achieving trajectory goal before or after commanded time. + If set to zero, the controller will wait a potentially infinite amount of time.", validation: { - lower_bounds: [0.0], + gt_eq: [0.0], } } __map_joints: diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index f28703efc9..22d43e1bfb 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -16,11 +16,12 @@ #include +#include "angles/angles.h" #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "rcppmath/clamp.hpp" #include "std_msgs/msg/header.hpp" + namespace joint_trajectory_controller { Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} @@ -44,10 +45,39 @@ Trajectory::Trajectory( void Trajectory::set_point_before_trajectory_msg( const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point) + const trajectory_msgs::msg::JointTrajectoryPoint & current_point, + const std::vector & joints_angle_wraparound) { time_before_traj_msg_ = current_time; state_before_traj_msg_ = current_point; + + // Compute offsets due to wrapping joints + wraparound_joint( + state_before_traj_msg_.positions, trajectory_msg_->points[0].positions, + joints_angle_wraparound); +} + +void wraparound_joint( + std::vector & current_position, const std::vector next_position, + const std::vector & joints_angle_wraparound) +{ + double dist; + // joints_angle_wraparound is even empty, or has the same size as the number of joints + for (size_t i = 0; i < joints_angle_wraparound.size(); i++) + { + if (joints_angle_wraparound[i]) + { + dist = angles::shortest_angular_distance(current_position[i], next_position[i]); + + // Deal with singularity at M_PI shortest distance + if (std::abs(std::abs(dist) - M_PI) < 1e-9) + { + dist = next_position[i] > current_position[i] ? std::abs(dist) : -std::abs(dist); + } + + current_position[i] = next_position[i] - dist; + } + } } void Trajectory::update(std::shared_ptr joint_trajectory) @@ -55,6 +85,7 @@ void Trajectory::update(std::shared_ptr j trajectory_msg_ = joint_trajectory; trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); sampled_already_ = false; + last_sample_idx_ = 0; } bool Trajectory::sample( @@ -64,7 +95,6 @@ bool Trajectory::sample( TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) - output_state = trajectory_msgs::msg::JointTrajectoryPoint(); if (trajectory_msg_->points.empty()) { @@ -90,6 +120,7 @@ bool Trajectory::sample( return false; } + output_state = trajectory_msgs::msg::JointTrajectoryPoint(); auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + first_point_in_msg.time_from_start; @@ -120,7 +151,7 @@ bool Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (size_t i = 0; i < last_idx; ++i) + for (size_t i = last_sample_idx_; i < last_idx; ++i) { auto & point = trajectory_msg_->points[i]; auto & next_point = trajectory_msg_->points[i + 1]; @@ -144,8 +175,9 @@ bool Trajectory::sample( interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); + start_segment_itr = begin() + static_cast(i); + end_segment_itr = begin() + static_cast(i + 1); + last_sample_idx_ = i; return true; } } @@ -153,6 +185,7 @@ bool Trajectory::sample( // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); + last_sample_idx_ = last_idx; output_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them if (output_state.velocities.empty()) @@ -351,4 +384,9 @@ rclcpp::Time Trajectory::time_from_start() const { return trajectory_start_time_ bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } +bool Trajectory::has_nontrivial_msg() const +{ + return has_trajectory_msg() && trajectory_msg_->points.size() > 1; +} + } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml deleted file mode 100644 index 09a134e06a..0000000000 --- a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml +++ /dev/null @@ -1,12 +0,0 @@ -test_joint_trajectory_controller: - joints: - - joint1 - - joint2 - - joint3 - - command_interfaces: - - position - - state_interfaces: - - position - - velocity diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 0f94d0b16c..eb1a3691e6 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -35,8 +35,10 @@ TEST(TestLoadJointStateController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController")); + ASSERT_NE( + cm.load_controller( + "test_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController"), + nullptr); rclcpp::shutdown(); } diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp new file mode 100644 index 0000000000..bd6304df29 --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -0,0 +1,441 @@ +// Copyright 2024 Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_trajectory_controller_utils.hpp" + +using joint_trajectory_controller::SegmentTolerances; +using trajectory_msgs::msg::JointTrajectoryPoint; + +std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + +control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( + const std::vector & points, double goal_time_tolerance, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) +{ + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return goal_msg; +} +class TestTolerancesFixture : public ::testing::Test +{ +protected: + SegmentTolerances default_tolerances; + joint_trajectory_controller::Params params; + std::vector joint_names_; + rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); + + void SetUp() override + { + // Initialize joint_names_ with some test data + joint_names_ = {"joint1", "joint2", "joint3"}; + + // Initialize default_tolerances and params with common setup for all tests + // TODO(anyone) fill params and use + // SegmentTolerances get_segment_tolerances(Params const & params) instead + default_tolerances.goal_time_tolerance = default_goal_time; + default_tolerances.state_tolerance.resize(joint_names_.size()); + default_tolerances.goal_state_tolerance.resize(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + default_tolerances.state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).velocity = stopped_velocity_tolerance; + } + params.joints = joint_names_; + } + + void TearDown() override + { + // Cleanup code if necessary + } +}; + +TEST_F(TestTolerancesFixture, test_get_segment_tolerances) +{ + // send goal with nonzero tolerances, are they accepted? + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); +} + +// send goal with deactivated tolerances (-1) +TEST_F(TestTolerancesFixture, test_deactivate_tolerances) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +// send goal with invalid tolerances, are the default ones used? +TEST_F(TestTolerancesFixture, test_invalid_tolerances) +{ + { + SCOPED_TRACE("negative goal_time_tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } +} +TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); +} +TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); +} diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index fbf198300b..eb12cda175 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -47,10 +47,12 @@ TEST(TestTrajectory, initialize_trajectory) empty_msg->header.stamp.nanosec = 2; const rclcpp::Time empty_time = empty_msg->header.stamp; auto traj = joint_trajectory_controller::Trajectory(empty_msg); + EXPECT_EQ(0, traj.last_sample_index()); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -67,6 +69,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -112,6 +115,7 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -124,6 +128,7 @@ TEST(TestTrajectory, sample_trajectory_positions) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -132,6 +137,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -145,6 +151,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -157,6 +164,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -168,6 +176,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -177,6 +186,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -185,6 +195,19 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample long past given points for same trajectory, it should receive the same end point + // so later in the query_state_service we set it to failure + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -342,6 +365,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.last_sample_index(), 0); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -354,6 +378,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -362,6 +387,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -382,6 +408,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -396,6 +423,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -416,6 +444,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -430,6 +459,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -450,6 +480,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -463,6 +494,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -515,6 +547,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -528,6 +561,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -536,6 +570,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -556,6 +591,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -607,6 +643,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -620,6 +657,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -634,6 +672,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -650,6 +689,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -666,6 +706,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -678,6 +719,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -724,6 +766,7 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { traj.sample(time_now, no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -739,6 +782,7 @@ TEST(TestTrajectory, skip_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -747,6 +791,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -763,6 +808,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -778,6 +824,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -788,6 +835,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -796,6 +844,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -804,9 +853,142 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } } } + +TEST(TestTrajectory, update_trajectory) +{ + // Verify that sampling works correctly after updating with a new trajectory + auto first_msg = std::make_shared(); + first_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + first_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + first_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + first_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, first_msg); + EXPECT_EQ(0, traj.last_sample_index()); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // Sample at starting time + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + + // Sample 2.5s after msg + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(1, traj.last_sample_index()); + + // Update trajectory + auto snd_msg = std::make_shared(); + snd_msg->header.stamp = rclcpp::Time(0); + + snd_msg->points.push_back(p1); + snd_msg->points.push_back(p2); + snd_msg->points.push_back(p3); + + traj.update(snd_msg); + + // Sample at starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR( + (p1.positions[0] - point_before_msg.positions[0]), expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(std::next(traj.begin()), end); + EXPECT_NEAR((p1.positions[0] + p2.positions[0]) / 2, expected_state.positions[0], EPS); + } +} + +TEST(TestWrapAroundJoint, no_wraparound) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector joints_angle_wraparound(3, false); + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], initial_position[0]); + EXPECT_EQ(current_position[1], initial_position[1]); + EXPECT_EQ(current_position[2], initial_position[2]); +} + +TEST(TestWrapAroundJoint, wraparound_single_joint) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector joints_angle_wraparound{true, false, false}; + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI); + EXPECT_EQ(current_position[1], initial_position[1]); + EXPECT_EQ(current_position[2], initial_position[2]); +} + +TEST(TestWrapAroundJoint, wraparound_all_joints) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector joints_angle_wraparound(3, true); + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI); + EXPECT_EQ(current_position[1], initial_position[1] + 2 * M_PI); + EXPECT_EQ(current_position[2], initial_position[2] + 2 * M_PI); +} + +TEST(TestWrapAroundJoint, wraparound_all_joints_no_offset) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(next_position); + std::vector joints_angle_wraparound(3, true); + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], next_position[0]); + EXPECT_EQ(current_position[1], next_position[1]); + EXPECT_EQ(current_position[2], next_position[2]); +} diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 10dd3a6b90..5f788343e1 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -27,12 +27,10 @@ #include #include -#include "action_msgs/msg/goal_status_array.hpp" #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" #include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -45,10 +43,12 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; @@ -66,21 +66,20 @@ class TestTrajectoryActions : public TrajectoryControllerTest goal_options_.feedback_callback = nullptr; } - void SetUpExecutor(const std::vector & parameters = {}) + void SetUpExecutor( + const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - executor_ = std::make_unique(); - - SetUpAndActivateTrajectoryController(true, parameters); - - executor_->add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpAndActivateTrajectoryController( + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); - executor_->add_node(node_->get_node_base_interface()); + executor_.add_node(node_->get_node_base_interface()); - executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_->spin(); }); + executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_.spin(); }); } void SetUpControllerHardware() @@ -125,6 +124,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest { TearDownControllerHardware(); TearDownExecutor(); + TrajectoryControllerTest::TearDown(); } void TearDownExecutor() @@ -132,7 +132,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest if (setup_executor_) { setup_executor_ = false; - executor_->cancel(); + executor_.cancel(); executor_future_handle_.wait(); } } @@ -154,10 +154,17 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt) + const std::vector & points, double goal_time_tolerance, + const GoalOptions & opt, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; @@ -169,7 +176,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; bool setup_executor_ = false; - rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_; + rclcpp::executors::MultiThreadedExecutor executor_; std::future executor_future_handle_; bool setup_controller_hw_ = false; @@ -201,22 +208,77 @@ class TestTrajectoryActions : public TrajectoryControllerTest } }; -TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class TestTrajectoryActionsTestParameterized +: public TestTrajectoryActions, + public ::testing::WithParamInterface< + std::tuple, std::vector>> { - SetUpExecutor(); +public: + virtual void SetUp() + { + TestTrajectoryActions::SetUp(); + command_interface_types_ = std::get<0>(GetParam()); + state_interface_types_ = std::get<1>(GetParam()); + } + + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } +}; + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) +{ + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; // send goal + std::vector point_positions{1.0, 2.0, 3.0}; { std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); + point.positions = point_positions; - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(point_positions); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + std::vector point_velocities{1.0, 1.0, 1.0}; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions = point_positions; + point.velocities = point_velocities; points.push_back(point); @@ -227,14 +289,21 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ(1.0, joint_pos_[0]); - EXPECT_EQ(2.0, joint_pos_[1]); - EXPECT_EQ(3.0, joint_pos_[2]); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(point_positions, point_velocities); } -TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -246,24 +315,67 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) std::shared_future gh_future; // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); - point1.positions.resize(joint_names_.size()); - - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); - point2.positions.resize(joint_names_.size()); + point2.positions = points_positions.at(1); + points.push_back(point2); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1)); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions = points_positions.at(0); + point1.velocities = points_velocities.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions = points_positions.at(1); + point2.velocities = points_velocities.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -274,11 +386,18 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1), points_velocities.at(1)); } +/** + * Makes sense with position command interface only, + * because no integration to position state interface is implemented + */ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { // set tolerance parameters @@ -292,15 +411,14 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) std::shared_future gh_future; // send goal + std::vector> points_positions{{{1.0, 2.0, 3.0}}}; { std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; + point.positions = points_positions.at(0); points.push_back(point); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -312,11 +430,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(0)); } +/** + * Makes sense with position command interface only, + * because no integration to position state interface is implemented + */ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { // set tolerance parameters @@ -337,24 +462,21 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) std::shared_future gh_future; // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -367,12 +489,172 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1)); } -TEST_F(TestTrajectoryActions, test_state_tolerances_fail) +/** + * No need for parameterized tests + */ +TEST_F(TestTrajectoryActions, test_tolerances_via_actions) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1), + rclcpp::Parameter("constraints.goal_time", default_goal_time), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), + rclcpp::Parameter("constraints.joint1.trajectory", 0.1), + rclcpp::Parameter("constraints.joint2.trajectory", 0.1), + rclcpp::Parameter("constraints.joint3.trajectory", 0.1)}; + + SetUpExecutor(params); + + { + SCOPED_TRACE("Check default values"); + SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 0.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + expectDefaultTolerances(active_tolerances); + } + + // send goal with nonzero tolerances, are they accepted? + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 2.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); + } + + // send goal without tolerances again, are the default ones used? + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 0.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + expectDefaultTolerances(active_tolerances); + } +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters const double state_tol = 0.0001; @@ -381,29 +663,28 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint2.trajectory", state_tol), rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; - SetUpExecutor(params); + // separate command from states -> immediate state tolerance fail + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); std::shared_future gh_future; // send goal + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.0); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.1); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -416,15 +697,15 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(INITIAL_POS_JOINTS); } -TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) { // set joint tolerance parameters const double goal_tol = 0.1; @@ -436,15 +717,13 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) rclcpp::Parameter("constraints.joint3.goal", goal_tol), rclcpp::Parameter("constraints.goal_time", goal_time)}; - SetUpExecutor(params); + // separate command from states -> goal won't never be reached + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); - const double init_pos1 = joint_pos_[0]; - const double init_pos2 = joint_pos_[1]; - const double init_pos3 = joint_pos_[2]; - std::shared_future gh_future; - // send goal + // send goal; one point only -> command is directly set to reach this goal (no interpolation) { std::vector points; JointTrajectoryPoint point; @@ -466,15 +745,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(INITIAL_POS_JOINTS); } -TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) { // set joint tolerance parameters const double state_tol = 0.0001; @@ -483,13 +762,11 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) rclcpp::Parameter("constraints.joint2.trajectory", state_tol), rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; - SetUpExecutor(params); + // separate command from states -> goal won't never be reached + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); - const double init_pos1 = joint_pos_[0]; - const double init_pos2 = joint_pos_[1]; - const double init_pos3 = joint_pos_[2]; - std::shared_future gh_future; // send goal { @@ -513,15 +790,15 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(INITIAL_POS_JOINTS); } -TEST_F(TestTrajectoryActions, test_cancel_hold_position) +TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) { SetUpExecutor(); SetUpControllerHardware(); @@ -558,14 +835,271 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - const double prev_pos1 = joint_pos_[0]; - const double prev_pos2 = joint_pos_[1]; - const double prev_pos3 = joint_pos_[2]; + std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position, + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(cancelled_position); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) +{ + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal with nonzero last velocities + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 4.0; + point2.velocities[1] = 5.0; + point2.velocities[2] = 6.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + // will be accepted despite nonzero last point + EXPECT_TRUE(gh_future.get()); + if ((traj_controller_->has_effort_command_interface()) == false) + { + // can't succeed with effort cmd if + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + } +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) +{ + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal with nonzero last velocities + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 4.0; + point2.velocities[1] = 5.0; + point2.velocities[2] = 6.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_FALSE(gh_future.get()); + + // send goal with last velocity being zero + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); - // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 0.0; + point2.velocities[1] = 0.0; + point2.velocities[2] = 0.0; + points.push_back(point2); - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + + EXPECT_TRUE(gh_future.get()); } + +TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_action) +{ + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); + + // We use our own hardware thread here, as we want to make sure the controller is deactivated + auto controller_thread = std::thread( + [&]() + { + // controller hardware cycle update loop + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + auto now_time = clock.now(); + auto last_time = now_time; + rclcpp::Duration wait = rclcpp::Duration::from_seconds(0.5); + auto end_time = last_time + wait; + while (now_time < end_time) + { + now_time = now_time + rclcpp::Duration::from_seconds(0.01); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + RCLCPP_INFO(node_->get_logger(), "Controller hardware thread finished"); + traj_controller_->get_node()->deactivate(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + }); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(2.5); + point.positions = point_positions; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + + controller_thread.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + + auto state_ref = traj_controller_->get_state_reference(); + auto state = traj_controller_->get_state_feedback(); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint + // method. + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(state_ref.positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(state_ref.positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(state_ref.positions.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } +} + +// position controllers +INSTANTIATE_TEST_SUITE_P( + PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllersAction, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6b0020c294..e82c8bd033 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include #include #include @@ -25,14 +25,10 @@ #include #include -#include "gmock/gmock.h" - #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" -#include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -46,27 +42,21 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_trajectory_controller_utils.hpp" + using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } - -void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } - TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - - // const auto future_handle_ = std::async(std::launch::async, spin, &executor); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -102,79 +92,20 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, joint_names_); } TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor); - - // set command_joints parameter + // set command_joints parameter different than joint_names_ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); - traj_controller_->get_node()->set_parameter({command_joint_names_param}); + SetUpTrajectoryController(executor, {command_joint_names_param}); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names_); } TEST_P(TrajectoryControllerTestParameterized, activate) @@ -182,140 +113,28 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - traj_controller_->get_node()->configure(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - auto cmd_interface_config = traj_controller_->command_interface_configuration(); - ASSERT_EQ( - cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_names_.size() * command_interface_types_.size()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_interface_config = traj_controller_->state_interface_configuration(); - ASSERT_EQ( - state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_names_.size() * state_interface_types_.size()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } -// TEST_F(TestTrajectoryController, activation) { -// auto traj_controller = std::make_shared( -// joint_names_, op_mode_); -// auto ret = traj_controller->init(test_robot_, controller_name_); -// if (ret != controller_interface::return_type::OK) { -// FAIL(); -// } -// -// auto traj_node = traj_controller->get_node(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// -// auto state = traj_controller_->configure(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// -// // wait for the subscriber and publisher to completely setup -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points {{{3.3, 4.4, 5.5}}}; -// publish(time_from_start, points, rclcpp::Time()); -// // wait for msg is be published to the system -// std::this_thread::sleep_for(std::chrono::milliseconds(1000)); -// executor.spin_once(); -// -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // change in hw position -// EXPECT_EQ(3.3, joint_pos_[0]); -// EXPECT_EQ(4.4, joint_pos_[1]); -// EXPECT_EQ(5.5, joint_pos_[2]); -// -// executor.cancel(); -// } - -// TEST_F(TestTrajectoryController, reactivation) { -// auto traj_controller = std::make_shared( -// joint_names_, op_mode_); -// auto ret = traj_controller->init(test_robot_, controller_name_); -// if (ret != controller_interface::return_type::OK) { -// FAIL(); -// } -// -// auto traj_node = traj_controller->get_node(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// -// auto state = traj_controller_->configure(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// -// // wait for the subscriber and publisher to completely setup -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// // *INDENT-OFF* -// std::vector> points { -// {{3.3, 4.4, 5.5}}, -// {{7.7, 8.8, 9.9}}, -// {{10.10, 11.11, 12.12}} -// }; -// // *INDENT-ON* -// publish(time_from_start, points, rclcpp::Time()); -// // wait for msg is be published to the system -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// executor.spin_once(); -// -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // deactivated -// // wait so controller process the second point when deactivated -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// state = traj_controller_->deactivate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// resource_manager_->read(); -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // no change in hw position -// EXPECT_EQ(3.3, joint_pos_[0]); -// EXPECT_EQ(4.4, joint_pos_[1]); -// EXPECT_EQ(5.5, joint_pos_[2]); -// -// // reactivated -// // wait so controller process the third point when reactivated -// std::this_thread::sleep_for(std::chrono::milliseconds(3000)); -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// resource_manager_->read(); -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // change in hw position to 3rd point -// EXPECT_EQ(10.10, joint_pos_[0]); -// EXPECT_EQ(11.11, joint_pos_[1]); -// EXPECT_EQ(12.12, joint_pos_[2]); -// -// executor.cancel(); -// } - TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor); + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -338,13 +157,22 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - // update for 0.25 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - // should be home pose again - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + // configure controller + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + // cleanup controller + state = traj_controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); executor.cancel(); } @@ -352,17 +180,15 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor, false); + SetUpTrajectoryController(executor); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - SetParameters(); - traj_controller_->configure(); - auto state = traj_controller_->get_state(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ActivateTrajectoryController(); - - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -383,64 +209,51 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // first update traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - // wait so controller process the second point when deactivated + // wait for reaching the first point + // controller would process the second point when deactivated below traj_controller_->update( rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); - // deactivated - state = traj_controller_->get_node()->deactivate(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - - const auto allowed_delta = 0.05; + EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); - } - - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); + EXPECT_NEAR(points.at(0).at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(2), joint_pos_[2], COMMON_THRESHOLD); } - // cleanup - state = traj_controller_->get_node()->cleanup(); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + // deactivate + std::vector deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; + state = traj_controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + // it should be holding the current point + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectHoldingPointDeactivated(deactivated_positions); - // TODO(anyone): should the controller even allow calling update() when it is not active? - // update loop receives a new msg and updates accordingly - traj_controller_->update( - rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); + // reactivate + // wait so controller would have processed the third point when reactivated -> but it shouldn't + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + state = ActivateTrajectoryController(false, deactivated_positions); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // state = traj_controller_->get_node()->configure(); - // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + // it should still be holding the position at time of deactivation + // i.e., active but trivial trajectory (one point only) + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectCommandPoint(deactivated_positions); executor.cancel(); } -TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); - subscribeToState(); + SetUpAndActivateTrajectoryController(executor, {}); + subscribeToStateLegacy(); updateController(); // Spin to receive latest state executor.spin_some(); - auto state = getState(); + auto state = getStateLegacy(); size_t n_joints = joint_names_.size(); @@ -483,977 +296,1789 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } -void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { - rclcpp::Parameter state_publish_rate_param( - "state_publish_rate", static_cast(target_msg_count)); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param}); + SetUpAndActivateTrajectoryController(executor, {}); + subscribeToState(executor); + updateController(); - auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); - using control_msgs::msg::JointTrajectoryControllerState; + size_t n_joints = joint_names_.size(); - const int qos_level = 10; - int echo_received_counter = 0; - rclcpp::Subscription::SharedPtr subs = - traj_controller_->get_node()->create_subscription( - controller_name_ + "/state", qos_level, - [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_EQ(joint_names_[i], state->joint_names[i]); + } - // update for 1second - auto clock = rclcpp::Clock(RCL_STEADY_TIME); - const auto start_time = clock.now(); - const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); - const auto end_time = start_time + wait; - while (clock.now() < end_time) + // No trajectory by default, no reference state or error + EXPECT_TRUE( + state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); + EXPECT_TRUE( + state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); + + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + + // expect feedback including all state_interfaces + EXPECT_EQ(n_joints, state->feedback.positions.size()); + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + state_interface_types_.end()) { - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_TRUE(state->feedback.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->feedback.velocities.size()); + } + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->feedback.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->feedback.accelerations.size()); } - // We may miss the last message since time allowed is exactly the time needed - EXPECT_NEAR(target_msg_count, echo_received_counter, 1); + // expect output including all command_interfaces + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.positions.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.positions.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.velocities.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.accelerations.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.effort.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.effort.size()); + } +} + +/** + * @brief check if dynamic parameters are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateControllerAsync(); + auto pids = traj_controller_->get_pids(); + + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_EQ(pids.size(), 3); + auto gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, 0.0); + + double kp = 1.0; + SetPidParameters(kp); + updateControllerAsync(); + + pids = traj_controller_->get_pids(); + EXPECT_EQ(pids.size(), 3); + gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, kp); + } + else + { + // nothing to check here, skip further test + EXPECT_EQ(pids.size(), 0); + } executor.cancel(); } /** - * @brief test_state_publish_rate Test that state publish rate matches configure rate + * @brief check if dynamic tolerances are updated */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { - test_state_publish_rate_target(10); + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateControllerAsync(); + + // test default parameters + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 0.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01); + } + } + + // change parameters, update and see what happens + std::vector new_tolerances{ + rclcpp::Parameter("constraints.goal_time", 1.0), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02), + rclcpp::Parameter("constraints.joint1.trajectory", 1.0), + rclcpp::Parameter("constraints.joint2.trajectory", 2.0), + rclcpp::Parameter("constraints.joint3.trajectory", 3.0), + rclcpp::Parameter("constraints.joint1.goal", 10.0), + rclcpp::Parameter("constraints.joint2.goal", 20.0), + rclcpp::Parameter("constraints.joint3.goal", 30.0)}; + for (const auto & param : new_tolerances) + { + traj_controller_->get_node()->set_parameter(param); + } + updateControllerAsync(); + + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 1.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast(i) + 1.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast(i) + 1.0)); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02); + } + } + + executor.cancel(); } -// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -// { -// test_state_publish_rate_target(0); -// } - -// /** -// * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from -// * internal controller order -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// { -// trajectory_msgs::msg::JointTrajectory traj_msg; -// const std::vector jumbled_joint_names{ -// joint_names_[1], joint_names_[2], joint_names_[0]}; -// traj_msg.joint_names = jumbled_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(3); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 3.0; -// traj_msg.points[0].positions[2] = 1.0; - -// if (traj_controller_->has_velocity_command_interface()) -// { -// traj_msg.points[0].velocities.resize(3); -// traj_msg.points[0].velocities[0] = -0.1; -// traj_msg.points[0].velocities[1] = -0.1; -// traj_msg.points[0].velocities[2] = -0.1; -// } - -// if (traj_controller_->has_effort_command_interface()) -// { -// traj_msg.points[0].effort.resize(3); -// traj_msg.points[0].effort[0] = -0.1; -// traj_msg.points[0].effort[1] = -0.1; -// traj_msg.points[0].effort[2] = -0.1; -// } - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.25 seconds -// // TODO(destogl): Make this time a bit shorter to increase stability on the CI? -// // Currently COMMON_THRESHOLD is adjusted. -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// if (traj_controller_->has_position_command_interface()) -// { -// EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); -// EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); -// } - -// if (traj_controller_->has_velocity_command_interface()) -// { -// EXPECT_GT(0.0, joint_vel_[0]); -// EXPECT_GT(0.0, joint_vel_[1]); -// EXPECT_GT(0.0, joint_vel_[2]); -// } - -// if (traj_controller_->has_effort_command_interface()) -// { -// EXPECT_GT(0.0, joint_eff_[0]); -// EXPECT_GT(0.0, joint_eff_[1]); -// EXPECT_GT(0.0, joint_eff_[2]); -// } -// } - -// /** -// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled -// * joints -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - -// const double initial_joint1_cmd = joint_pos_[0]; -// const double initial_joint2_cmd = joint_pos_[1]; -// const double initial_joint3_cmd = joint_pos_[2]; -// trajectory_msgs::msg::JointTrajectory traj_msg; - -// { -// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; -// traj_msg.joint_names = partial_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(2); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 1.0; -// traj_msg.points[0].velocities.resize(2); -// traj_msg.points[0].velocities[0] = -// copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); -// traj_msg.points[0].velocities[1] = -// copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.5 seconds -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// double threshold = 0.001; - -// if (traj_controller_->has_position_command_interface()) -// { -// EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); -// EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); -// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) -// << "Joint 3 command should be current position"; -// } - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != -// command_interface_types_.end()) -// { -// // estimate the sign of the velocity -// // joint rotates forward -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); -// EXPECT_NEAR(0.0, joint_vel_[2], threshold) -// << "Joint 3 velocity should be 0.0 since it's not in the goal"; -// } - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != -// command_interface_types_.end()) -// { -// // estimate the sign of the velocity -// // joint rotates forward -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); -// EXPECT_NEAR(0.0, joint_eff_[2], threshold) -// << "Joint 3 effort should be 0.0 since it's not in the goal"; -// } -// // TODO(anyone): add here ckecks for acceleration commands - -// executor.cancel(); -// } - -// /** -// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled -// * joints without allow_partial_joints_goal -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); - -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - -// const double initial_joint1_cmd = joint_pos_[0]; -// const double initial_joint2_cmd = joint_pos_[1]; -// const double initial_joint3_cmd = joint_pos_[2]; -// const double initial_joint_vel = 0.0; -// const double initial_joint_acc = 0.0; -// trajectory_msgs::msg::JointTrajectory traj_msg; - -// { -// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; -// traj_msg.joint_names = partial_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(2); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 1.0; -// traj_msg.points[0].velocities.resize(2); -// traj_msg.points[0].velocities[0] = 2.0; -// traj_msg.points[0].velocities[1] = 1.0; - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.5 seconds -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// double threshold = 0.001; -// EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) -// << "All joints command should be current position because goal was rejected"; -// EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) -// << "All joints command should be current position because goal was rejected"; -// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) -// << "All joints command should be current position because goal was rejected"; - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != -// command_interface_types_.end()) -// { -// EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// } - -// if ( -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != -// command_interface_types_.end()) -// { -// EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// } - -// executor.cancel(); -// } - -// /** -// * @brief invalid_message Test mismatched joint and reference vector sizes -// */ -// TEST_P(TrajectoryControllerTestParameterized, invalid_message) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); -// rclcpp::Parameter allow_integration_parameters( -// "allow_integration_in_goal_trajectories", false); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController( -// true, {partial_joints_parameters, allow_integration_parameters}, &executor); - -// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - -// good_traj_msg.joint_names = joint_names_; -// good_traj_msg.header.stamp = rclcpp::Time(0); -// good_traj_msg.points.resize(1); -// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// good_traj_msg.points[0].positions.resize(1); -// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; -// good_traj_msg.points[0].velocities.resize(1); -// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - -// // Incompatible joint names -// traj_msg = good_traj_msg; -// traj_msg.joint_names = {"bad_name"}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // No position data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too many positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few velocities -// traj_msg = good_traj_msg; -// traj_msg.points[0].velocities = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few accelerations -// traj_msg = good_traj_msg; -// traj_msg.points[0].accelerations = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few efforts -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].effort = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Non-strictly increasing waypoint times -// traj_msg = good_traj_msg; -// traj_msg.points.push_back(traj_msg.points.front()); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -// } - -// /// With allow_integration_in_goal_trajectories parameter trajectory missing position or -// /// velocities are accepted -// TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) -// { -// rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); - -// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - -// good_traj_msg.joint_names = joint_names_; -// good_traj_msg.header.stamp = rclcpp::Time(0); -// good_traj_msg.points.resize(1); -// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// good_traj_msg.points[0].positions.resize(1); -// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; -// good_traj_msg.points[0].velocities.resize(1); -// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; -// good_traj_msg.points[0].accelerations.resize(1); -// good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - -// // No position data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // No position and velocity data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].velocities.clear(); -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // All empty -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].velocities.clear(); -// traj_msg.points[0].accelerations.clear(); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too many positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few velocities -// traj_msg = good_traj_msg; -// traj_msg.points[0].velocities = {1.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few accelerations -// traj_msg = good_traj_msg; -// traj_msg.points[0].accelerations = {2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -// } - -// /** -// * @brief test_trajectory_replace Test replacing an existing trajectory -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - -// subscribeToState(); - -// std::vector> points_old{{{2., 3., 4.}}}; -// std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; -// std::vector> points_partial_new{{1.5}}; -// std::vector> points_partial_new_velocities{{0.15}}; - -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_actual.velocities = { -// points_old_velocities[0].begin(), points_old_velocities[0].end()}; -// expected_desired.velocities = { -// points_old_velocities[0].begin(), points_old_velocities[0].end()}; -// // Check that we reached end of points_old trajectory -// // Denis: delta was 0.1 with 0.2 works for me -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - -// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); -// points_partial_new_velocities[0][0] = -// copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); -// publish( -// time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); - -// // Replaced trajectory is a mix of previous and current goal -// expected_desired.positions[0] = points_partial_new[0][0]; -// expected_desired.positions[1] = points_old[0][1]; -// expected_desired.positions[2] = points_old[0][2]; -// expected_desired.velocities[0] = points_partial_new_velocities[0][0]; -// expected_desired.velocities[1] = 0.0; -// expected_desired.velocities[2] = 0.0; -// expected_actual = expected_desired; -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } - -// /** -// * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); - -// // TODO(anyone): add expectations for velocities and accelerations -// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; -// std::vector> points_new{{{-1., -2., -3.}}}; - -// RCLCPP_INFO( -// traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time()); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0] trajectory -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); -// // New trajectory will end before current time -// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); -// expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; -// expected_desired = expected_actual; -// std::cout << "Sending old trajectory" << std::endl; -// publish(time_from_start, points_new, new_traj_start); -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } - -// TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); - -// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; -// std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time()); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0]trajectory -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// RCLCPP_INFO( -// traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); -// // New trajectory first point is in the past, second is in the future -// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); -// expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; -// expected_desired = expected_actual; -// publish(time_from_start, points_new, new_traj_start); -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } - -// TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) -// { -// SetUpTrajectoryController(); -// auto traj_node = traj_controller_->get_node(); -// RCLCPP_WARN( -// traj_node->get_logger(), -// "Test disabled until current_trajectory is taken into account when adding a new trajectory."); -// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ -// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 -// return; - -// // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// subscribeToState(); -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// traj_node->set_parameter(partial_joints_parameters); -// traj_controller_->get_node()->configure(); -// traj_controller_->get_node()->activate(); - -// std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; -// std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; -// std::vector> partial_traj{ -// {{-1., -2.}, -// { -// -2., -// -4, -// }}}; -// std::vector> partial_traj_velocities{ -// {{-0.1, -0.2}, -// { -// -0.2, -// -0.4, -// }}}; -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; -// // Send full trajectory -// publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); -// // Sleep until first waypoint of full trajectory - -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0]trajectory and are starting points_old[1] -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// // Send partial trajectory starting after full trajecotry is complete -// RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); -// publish( -// points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); -// // Wait until the end start and end of partial traj - -// expected_actual.positions = { -// partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; -// expected_desired = expected_actual; - -// waitAndCompareState( -// expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); -// } - -// TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// // goal setup -// std::vector first_goal = {3.3, 4.4, 5.5}; -// std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; -// std::vector second_goal = {6.6, 8.8, 11.0}; -// std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; -// double state_from_command_offset = 0.3; - -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points{{first_goal}}; -// publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); -// traj_controller_->wait_for_trajectory(executor); -// updateController(rclcpp::Duration::from_seconds(1.1)); - -// if (traj_controller_->has_position_command_interface()) -// { -// // JTC is executing trajectory in open-loop therefore: -// // - internal state does not have to be updated (in this test-case it shouldn't) -// // - internal command is updated -// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - -// // Move joint further in the same direction as before (to the second goal) -// points = {{second_goal}}; -// publish(time_from_start, points, rclcpp::Time(1.0), {}, second_goal_velocities); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there should be a "jump" in opposite direction from command -// // (towards the state value) -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // Expect backward commands at first -// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); - -// // Finally the second goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - -// // Move joint back to the first goal -// points = {{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there should be a "jump" in the goal direction from command -// // (towards the state value) -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // Expect backward commands at first -// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); - -// // Finally the first goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// } - -// executor.cancel(); -// } - -// TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// // goal setup -// std::vector first_goal = {3.3, 4.4, 5.5}; -// std::vector second_goal = {6.6, 8.8, 11.0}; -// double state_from_command_offset = 0.3; - -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points{{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); -// updateController(rclcpp::Duration::from_seconds(1.1)); - -// if (traj_controller_->has_position_command_interface()) -// { -// // JTC is executing trajectory in open-loop therefore: -// // - internal state does not have to be updated (in this test-case it shouldn't) -// // - internal command is updated -// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - -// // Move joint further in the same direction as before (to the second goal) -// points = {{second_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there **should not** be a "jump" in opposite direction from -// // command (towards the state value) -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // There should not be backward commands -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); - -// // Finally the second goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - -// // Move joint back to the first goal -// points = {{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there **should not** be a "jump" in the goal direction from -// // command (towards the state value) -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // There should not be a jump toward commands -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); - -// // Finally the first goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// } - -// executor.cancel(); -// } - -// // Testing that values are read from state interfaces when hardware is started for the first -// // time and hardware state has offset --> this is indicated by NaN values in state interfaces -// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - -// // set command values to NaN -// for (size_t i = 0; i < 3; ++i) -// { -// joint_pos_[i] = std::numeric_limits::quiet_NaN(); -// joint_vel_[i] = std::numeric_limits::quiet_NaN(); -// joint_acc_[i] = std::numeric_limits::quiet_NaN(); -// } -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); - -// for (size_t i = 0; i < 3; ++i) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - -// // check velocity -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); -// } - -// // check acceleration -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); -// } -// } - -// executor.cancel(); -// } - -// // Testing that values are read from state interfaces when hardware is started after some values -// // are set on the hardware commands -// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - -// // set command values to NaN -// for (size_t i = 0; i < 3; ++i) -// { -// joint_pos_[i] = 3.1 + i; -// joint_vel_[i] = 0.25 + i; -// joint_acc_[i] = 0.02 + i / 10.0; -// } -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); - -// for (size_t i = 0; i < 3; ++i) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - -// // check velocity -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); -// } - -// // check acceleration -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); -// } -// } - -// executor.cancel(); -// } +/** + * @brief check if hold on startup + */ +TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; -// position controllers -INSTANTIATE_TEST_SUITE_P( - PositionTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple(std::vector({"position"}), std::vector({"position"})), - std::make_tuple( - std::vector({"position"}), std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"position"}), - std::vector({"position", "velocity", "acceleration"})))); + SetUpAndActivateTrajectoryController(executor, {}); -// position_velocity controllers -INSTANTIATE_TEST_SUITE_P( - PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"position", "velocity"}), std::vector({"position"})), - std::make_tuple( - std::vector({"position", "velocity"}), - std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"position", "velocity"}), - std::vector({"position", "velocity", "acceleration"})))); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + // after startup, we expect an active trajectory: + ASSERT_TRUE(traj_controller_->has_active_traj()); + // one point, being the position at startup + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectCommandPoint(initial_positions); -// position_velocity_acceleration controllers -INSTANTIATE_TEST_SUITE_P( - PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"position", "velocity", "acceleration"}), - std::vector({"position"})), - std::make_tuple( - std::vector({"position", "velocity", "acceleration"}), - std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"position", "velocity", "acceleration"}), - std::vector({"position", "velocity", "acceleration"})))); + executor.cancel(); +} -// // only velocity controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity", "acceleration"})))); - -// // only effort controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"effort"}), std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"effort"}), -// std::vector({"position", "velocity", "acceleration"})))); - -// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` -// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -// { -// auto set_parameter_and_check_result = [&]() -// { -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// SetParameters(); // This call is replacing the way parameters are set via launch -// traj_controller_->get_node()->configure(); -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// }; -// -// SetUpTrajectoryController(false); -// -// // command interfaces: empty -// command_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // command interfaces: bad_name -// command_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // command interfaces: effort has to be only -// command_interface_types_ = {"effort", "position"}; -// set_parameter_and_check_result(); -// -// // command interfaces: velocity - position not present -// command_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // command interfaces: acceleration without position and velocity -// command_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: empty -// state_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // state interfaces: cannot not be effort -// state_interface_types_ = {"effort"}; -// set_parameter_and_check_result(); -// -// // state interfaces: bad name -// state_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // state interfaces: velocity - position not present -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: acceleration without position and velocity -// state_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // velocity-only command interface: position - velocity not present -// command_interface_types_ = {"velocity"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// -// // effort-only command interface: position - velocity not present -// command_interface_types_ = {"effort"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// } +// Floating-point value comparison threshold +const double EPS = 1e-6; +/** + * @brief check if position error of revolute joints are wrapped around if not configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + bool angle_wraparound = false; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound); + subscribeToState(executor); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time()); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // no normalization of position error + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are wrapped around if configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // is error.positions[2] wrapped around? + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for joint0 and joint1 + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] wrapped around? + EXPECT_GT(0.0, joint_vel_[2]); // direction change because of angle wrap + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) for joint0 and joint1 + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] wrapped around? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + + executor.cancel(); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, accept_correct_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + double cmd_timeout = 3.0; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(cmd_timeout, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 1.0); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(0.0, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief check if no timeout is triggered + */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync +TEST_P(TrajectoryControllerTestParameterized, no_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + + // is the trajectory still active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should still hold the points from above + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + EXPECT_NEAR(state_reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference.positions[2], points.at(2).at(2), 1e-2); + // value of velocities is different from above due to spline interpolation + EXPECT_GT(state_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); + + executor.cancel(); +} + +/** + * @brief check if timeout is triggered + */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync +TEST_P(TrajectoryControllerTestParameterized, timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double cmd_timeout = 0.1; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // update until end of trajectory -> no timeout should have occurred + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3); + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should have the trajectory with three points + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + + // update until timeout should have happened + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // after timeout, set_hold_position adds a new trajectory + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should be not more than one point now (from hold position) + EXPECT_FALSE(traj_controller_->has_nontrivial_traj()); + // should hold last position with zero velocity + if (traj_controller_->has_position_command_interface()) + { + expectCommandPoint(points.at(2)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectCommandPoint(INITIAL_POS_JOINTS); + } + + executor.cancel(); +} + +void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) +{ + rclcpp::Parameter state_publish_rate_param( + "state_publish_rate", static_cast(target_msg_count)); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {state_publish_rate_param}); + + auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); + + using control_msgs::msg::JointTrajectoryControllerState; + + const int qos_level = 10; + int echo_received_counter = 0; + rclcpp::Subscription::SharedPtr subs = + traj_controller_->get_node()->create_subscription( + controller_name_ + "/state", qos_level, + [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + + // update for 1second + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); + const auto end_time = start_time + wait; + while (clock.now() < end_time) + { + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + } + + // We may miss the last message since time allowed is exactly the time needed + EXPECT_NEAR(target_msg_count, echo_received_counter, 2); + + executor.cancel(); +} + +/** + * @brief test_state_publish_rate Test that state publish rate matches configure rate + */ +TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +{ + test_state_publish_rate_target(10); +} + +TEST_P(TrajectoryControllerTestParameterized, test_lower_state_publish_rate) +{ + test_state_publish_rate_target(1); +} + +/** + * @brief check if use_closed_loop_pid is active + */ +TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + if ( + (traj_controller_->has_velocity_command_interface() && + !traj_controller_->has_position_command_interface() && + !traj_controller_->has_effort_command_interface() && + !traj_controller_->has_acceleration_command_interface() && + !traj_controller_->is_open_loop()) || + traj_controller_->has_effort_command_interface()) + { + EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + } +} + +/** + * @brief check if velocity error is calculated correctly + */ +TEST_P(TrajectoryControllerTestParameterized, velocity_error) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points_positions{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.1, 0.1, 0.1}}, {{0.2, 0.2, 0.2}}, {{0.3, 0.3, 0.3}}}; + // *INDENT-ON* + publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(n_joints, state_reference.velocities.size()); + EXPECT_EQ(n_joints, state_feedback.velocities.size()); + EXPECT_EQ(n_joints, state_error.velocities.size()); + } + if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(n_joints, state_reference.accelerations.size()); + EXPECT_EQ(n_joints, state_feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_error.accelerations.size()); + } + + // no change in state interface should happen + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(state_feedback.velocities, INITIAL_VEL_JOINTS); + } + // is the velocity error correct? + if ( + traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller + || (traj_controller_->has_velocity_state_interface() && + traj_controller_->has_velocity_command_interface())) + { + // don't check against a value, because spline interpolation might overshoot depending on + // interface combinations + EXPECT_GE(state_error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_error.velocities[2], points_velocities[0][2]); + } + + executor.cancel(); +} + +/** + * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from + * internal controller order + */ +TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor); + std::vector points_positions = {1.0, 2.0, 3.0}; + std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; + { + trajectory_msgs::msg::JointTrajectory traj_msg; + const std::vector jumbled_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]], joint_names_[jumble_map[2]]}; + + traj_msg.joint_names = jumbled_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); + traj_msg.points[0].positions.resize(3); + traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); + traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); + traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); + traj_msg.points[0].velocities.resize(3); + traj_msg.points[0].accelerations.resize(3); + + for (size_t dof = 0; dof < 3; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } + + trajectory_publisher_->publish(traj_msg); + } + + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(points_positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling + // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with + // feedforward term only + EXPECT_GT(0.0, joint_vel_[0]); + EXPECT_GT(0.0, joint_vel_[1]); + EXPECT_GT(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + // effort should be nonzero, because we use PID with feedforward term + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } +} + +/** + * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled + * joints + */ +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); + + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; + const double initial_joint3_cmd = joint_pos_[2]; + const double dt = 0.25; + trajectory_msgs::msg::JointTrajectory traj_msg; + + { + std::vector jumble_map = {1, 0}; + std::vector partial_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].accelerations.resize(2); + for (size_t dof = 0; dof < 2; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } + + trajectory_publisher_->publish(traj_msg); + } + + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) + << "Joint 3 command should be current position"; + } + + if (traj_controller_->has_velocity_command_interface()) + { + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); + EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) + << "Joint 3 velocity should be 0.0 since it's not in the goal"; + } + + if (traj_controller_->has_acceleration_command_interface()) + { + // estimate the sign of the acceleration + // joint rotates forward + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) + << "Joint 3 acc should be 0.0 since it's not in the goal"; + } + + if (traj_controller_->has_effort_command_interface()) + { + // estimate the sign of the effort + // joint rotates forward + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } + + executor.cancel(); +} + +/** + * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled + * joints without allow_partial_joints_goal + */ +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); + + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; + const double initial_joint3_cmd = joint_pos_[2]; + trajectory_msgs::msg::JointTrajectory traj_msg; + + { + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].velocities[0] = 2.0; + traj_msg.points[0].velocities[1] = 1.0; + + trajectory_publisher_->publish(traj_msg); + } + + traj_controller_->wait_for_trajectory(executor); + // update for 0.5 seconds + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) + << "All joints command should be current position because goal was rejected"; + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) + << "All joints velocities should be 0.0 because goal was rejected"; + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) + << "All joints accelerations should be 0.0 because goal was rejected"; + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) + << "All joints efforts should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) + << "All joints efforts should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) + << "All joints efforts should be 0.0 because goal was rejected"; + } + + executor.cancel(); +} + +/** + * @brief invalid_message Test mismatched joint and reference vector sizes + */ +TEST_P(TrajectoryControllerTestParameterized, invalid_message) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController( + executor, {partial_joints_parameters, allow_integration_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // Incompatible joint names + traj_msg = good_traj_msg; + traj_msg.joint_names = {"bad_name"}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // empty message + traj_msg = good_traj_msg; + traj_msg.points.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Effort is not supported in trajectory message + traj_msg = good_traj_msg; + traj_msg.points[0].effort = {1.0, 2.0, 3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Non-strictly increasing waypoint times + traj_msg = good_traj_msg; + traj_msg.points.push_back(traj_msg.points.front()); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // End time in the past + traj_msg = good_traj_msg; + traj_msg.header.stamp = rclcpp::Time(1); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // End time in the future + traj_msg = good_traj_msg; + traj_msg.header.stamp = traj_controller_->get_node()->now(); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(10); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief Test invalid velocity at trajectory end with parameter set to false + */ +TEST_P( + TrajectoryControllerTestParameterized, + expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) +{ + rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg; + traj_msg.joint_names = joint_names_; + traj_msg.header.stamp = rclcpp::Time(0); + + // empty message (no throw!) + ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Nonzero velocity at trajectory end! + traj_msg.points.resize(1); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocities are accepted + */ +TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) +{ + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + good_traj_msg.points[0].accelerations.resize(1); + good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position and velocity data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // All empty + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + traj_msg.points[0].accelerations.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief test_trajectory_replace Test replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); + + std::vector> points_old{{{2., 3., 4.}}}; + std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; + std::vector> points_partial_new{{1.5}}; + std::vector> points_partial_new_velocities{{0.15}}; + + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; + expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + // Check that we reached end of points_old trajectory + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); + points_partial_new_velocities[0][0] = + std::copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + + // Replaced trajectory is a mix of previous and current goal + expected_desired.positions[0] = points_partial_new[0][0]; + expected_desired.positions[1] = points_old[0][1]; + expected_desired.positions[2] = points_old[0][2]; + expected_desired.velocities[0] = points_partial_new_velocities[0][0]; + expected_desired.velocities[1] = 0.0; + expected_desired.velocities[2] = 0.0; + expected_actual = expected_desired; + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); +} + +/** + * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + + // TODO(anyone): add expectations for velocities and accelerations + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}}}; + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time()); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0] trajectory + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); + // New trajectory will end before current time + rclcpp::Time new_traj_start = + rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; + expected_desired = expected_actual; + publish(time_from_start, points_new, new_traj_start); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); +} + +TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + + // send points_old and wait to reach first point + publish(time_from_start, points_old, rclcpp::Time()); + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + // send points_new before the old trajectory is finished + RCLCPP_INFO( + traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); + // New trajectory first point is in the past, second is in the future + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); + publish(time_from_start, points_new, new_traj_start); + // it should not have accepted the new goal but finish the old one + expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; + expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); +} + +TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); + + RCLCPP_WARN( + traj_controller_->get_node()->get_logger(), + "Test disabled until current_trajectory is taken into account when adding a new trajectory."); + // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ + // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 + return; + + // *INDENT-OFF* + std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; + std::vector> partial_traj{{{-1., -2.}, {-2., -4}}}; + std::vector> partial_traj_velocities{{{-0.1, -0.2}, {-0.2, -0.4}}}; + // *INDENT-ON* + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; + // Send full trajectory + publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); + // Sleep until first waypoint of full trajectory + + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory and are starting points_old[1] + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + // Send partial trajectory starting after full trajecotry is complete + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + publish( + points_delay, partial_traj, rclcpp::Clock(RCL_STEADY_TIME).now() + delay * 2, {}, + partial_traj_velocities); + // Wait until the end start and end of partial traj + + expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; + expected_desired = expected_actual; + + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); +} + +TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set parameter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; + std::vector second_goal = {6.6, 8.8, 11.0}; + std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); + std::vector> points{{first_goal}}; + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); + + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance + EXPECT_NEAR( + joint_state_pos_[0] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + 0.1); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // set open loop to true, this should change behavior from above + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector second_goal = {6.6, 8.8, 11.0}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); + std::vector> points{{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); + + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + executor.cancel(); +} + +// Testing that values are read from state interfaces when hardware is started for the first +// time and hardware state has offset --> this is indicated by NaN values in command interfaces +TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to NaN + std::vector initial_pos_cmd(3, std::numeric_limits::quiet_NaN()); + std::vector initial_vel_cmd(3, std::numeric_limits::quiet_NaN()); + std::vector initial_acc_cmd(3, std::numeric_limits::quiet_NaN()); + + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from state interfaces + // (command interface are NaN) + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (size_t i = 0; i < 3; ++i) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + } + + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + } + } + + executor.cancel(); +} + +// Testing that values are read from command interfaces when hardware is started after some values +// are set on the hardware commands +TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to arbitrary values + std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; + for (size_t i = 0; i < 3; ++i) + { + initial_pos_cmd.push_back(3.1 + static_cast(i)); + initial_vel_cmd.push_back(0.25 + static_cast(i)); + initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); + } + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from command interfaces + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (size_t i = 0; i < 3; ++i) + { + // check position + if (traj_controller_->has_position_command_interface()) + { + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + if (traj_controller_->has_velocity_command_interface()) + { + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + if (traj_controller_->has_acceleration_command_interface()) + { + // should have set it to last position + velocity + acceleration command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + } + } + else + { + // should have set it to last position + velocity command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + } + } + else + { + // should have set it to last position command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + } + } + + executor.cancel(); +} + +TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) +{ + // set joint tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; + + rclcpp::executors::MultiThreadedExecutor executor; + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, params, true, kp); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectCommandPoint(joint_state_pos_); +} + +TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) +{ + // set joint tolerance parameters + const double goal_tol = 0.1; + // set very small goal_time so that goal_time is violated + const double goal_time = 0.000001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", goal_tol), + rclcpp::Parameter("constraints.joint2.goal", goal_tol), + rclcpp::Parameter("constraints.joint3.goal", goal_tol), + rclcpp::Parameter("constraints.goal_time", goal_time)}; + + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, params, true, 1.0); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectCommandPoint(joint_state_pos_); + + // what happens if we wait longer but it harms the tolerance again? + auto hold_position = joint_state_pos_; + joint_state_pos_.at(0) = -3.3; + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); + // it should be still holding the old point + expectCommandPoint(hold_position); +} + +// position controllers +INSTANTIATE_TEST_SUITE_P( + PositionTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity_acceleration controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); + +/** + * @brief see if parameter validation is correct + * + * Note: generate_parameter_library validates parameters itself during on_init() method, but + * combinations of parameters are checked from JTC during on_configure() + */ +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ + // command interfaces: empty + command_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + auto state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + + // command interfaces: bad_name + command_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: effort has to be only + command_interface_types_ = {"effort", "position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: velocity - position not present + command_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: acceleration without position and velocity + command_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: empty + state_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: cannot not be effort + state_interface_types_ = {"effort"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: bad name + state_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: velocity - position not present + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + state_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: acceleration without position and velocity + state_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0d523cc88d..64ebf6f85a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,14 +21,16 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/tolerances.hpp" +#include "lifecycle_msgs/msg/state.hpp" namespace { -const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 0.001; const double INITIAL_POS_JOINT1 = 1.1; const double INITIAL_POS_JOINT2 = 2.1; const double INITIAL_POS_JOINT3 = 3.1; @@ -37,6 +39,47 @@ const std::vector INITIAL_POS_JOINTS = { const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; + +const double default_goal_time = 0.1; +const double stopped_velocity_tolerance = 0.1; + +[[maybe_unused]] void expectDefaultTolerances( + joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +bool is_same_sign_or_zero(double val1, double val2) +{ + return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); +} } // namespace namespace test_trajectory_controllers @@ -52,31 +95,24 @@ class TestableJointTrajectoryController const rclcpp_lifecycle::State & previous_state) override { auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); - // this class can still be useful without the wait set - if (joint_command_subscriber_) - { - joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); - } return ret; } /** - * @brief wait_for_trajectory block until a new JointTrajectory is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new JointTrajectory msg was received, false if timeout - */ - bool wait_for_trajectory( + * @brief wait_for_trajectory block until a new JointTrajectory is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_trajectory( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{10}) { - bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } void set_joint_names(const std::vector & joint_names) @@ -101,18 +137,57 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } - trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; } + bool has_position_state_interface() const { return has_position_state_interface_; } + + bool has_velocity_state_interface() const { return has_velocity_state_interface_; } + + bool has_acceleration_state_interface() const { return has_acceleration_state_interface_; } - bool has_position_command_interface() { return has_position_command_interface_; } + bool has_position_command_interface() const { return has_position_command_interface_; } - bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_velocity_command_interface() const { return has_velocity_command_interface_; } - bool has_effort_command_interface() { return has_effort_command_interface_; } + bool has_acceleration_command_interface() const { return has_acceleration_command_interface_; } - rclcpp::WaitSet joint_cmd_sub_wait_set_; + bool has_effort_command_interface() const { return has_effort_command_interface_; } + + bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } + + bool is_open_loop() const { return params_.open_loop_control; } + + joint_trajectory_controller::SegmentTolerances get_active_tolerances() + { + return *(active_tolerances_.readFromRT()); + } + + std::vector get_pids() const { return pids_; } + + joint_trajectory_controller::SegmentTolerances get_tolerances() const + { + return default_tolerances_; + } + + bool has_active_traj() const { return has_active_trajectory(); } + + bool has_trivial_traj() const + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; + } + + bool has_nontrivial_traj() + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + } + + double get_cmd_timeout() { return cmd_timeout_; } + + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } }; class TrajectoryControllerTest : public ::testing::Test @@ -143,35 +218,42 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); } - void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true) + void TearDown() override { - traj_controller_ = std::make_shared(); + DeactivateTrajectoryController(); + traj_controller_.reset(); + } - if (use_local_parameters) - { - traj_controller_->set_joint_names(joint_names_); - traj_controller_->set_command_interfaces(command_interface_types_); - traj_controller_->set_state_interfaces(state_interface_types_); - } - auto ret = traj_controller_->init(controller_name_); + void SetUpTrajectoryController( + rclcpp::Executor & executor, const std::vector & parameters = {}) + { + auto ret = SetUpTrajectoryControllerLocal(parameters); if (ret != controller_interface::return_type::OK) { FAIL(); } executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - SetParameters(); } - void SetParameters() + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}) { - auto node = traj_controller_->get_node(); - const rclcpp::Parameter joint_names_param("joints", joint_names_); - const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_); - const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); - node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); + traj_controller_ = std::make_shared(); + + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_)); + parameter_overrides.push_back( + rclcpp::Parameter("command_interfaces", command_interface_types_)); + parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return traj_controller_->init(controller_name_, "", node_options); } - void SetPidParameters() + void SetPidParameters( + double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -179,38 +261,56 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_p(prefix + ".p", p_value); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); + const rclcpp::Parameter angle_wraparound( + prefix + ".angle_wraparound", angle_wraparound_value); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); } } void SetUpAndActivateTrajectoryController( - rclcpp::Executor & executor, bool use_local_parameters = true, - const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + rclcpp::Executor & executor, const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + bool angle_wraparound = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - SetUpTrajectoryController(executor, use_local_parameters); - - // set pid parameters before configure - SetPidParameters(); - for (const auto & param : parameters) + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; }) != + parameters.end(); + + std::vector parameters_local = parameters; + if (!has_nonzero_vel_param) { - traj_controller_->get_node()->set_parameter(param); + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } - // ignore velocity tolerances for this test since they aren't committed in test_robot->write() - rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); - traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local); + // set pid parameters before configure + SetPidParameters(k_p, ff, angle_wraparound); traj_controller_->get_node()->configure(); - ActivateTrajectoryController(separate_cmd_and_state_values); + ActivateTrajectoryController( + separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, + initial_eff_joints); } - void ActivateTrajectoryController(bool separate_cmd_and_state_values = false) + rclcpp_lifecycle::State ActivateTrajectoryController( + bool separate_cmd_and_state_values = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -223,49 +323,94 @@ class TrajectoryControllerTest : public ::testing::Test acc_state_interfaces_.reserve(joint_names_.size()); for (size_t i = 0; i < joint_names_.size(); ++i) { - pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); - vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); - acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); - eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); - - pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], hardware_interface::HW_IF_POSITION, - separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); - vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], hardware_interface::HW_IF_VELOCITY, - separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); - acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], hardware_interface::HW_IF_ACCELERATION, - separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); + pos_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); + vel_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); + acc_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); + eff_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); + + pos_state_interfaces_.emplace_back( + hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, + separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); + vel_state_interfaces_.emplace_back( + hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, + separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); + acc_state_interfaces_.emplace_back( + hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_ACCELERATION, + separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_POS_JOINTS[i]); + cmd_interfaces.back().set_value(initial_pos_joints[i]); cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); + cmd_interfaces.back().set_value(initial_vel_joints[i]); cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]); + cmd_interfaces.back().set_value(initial_acc_joints[i]); cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]); - joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; - joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; - joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + cmd_interfaces.back().set_value(initial_eff_joints[i]); + if (separate_cmd_and_state_values) + { + joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; + joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; + joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + } state_interfaces.emplace_back(pos_state_interfaces_.back()); state_interfaces.emplace_back(vel_state_interfaces_.back()); state_interfaces.emplace_back(acc_state_interfaces_.back()); } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_node()->activate(); + return traj_controller_->get_node()->activate(); + } + + void DeactivateTrajectoryController() + { + if (traj_controller_) + { + if (traj_controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + EXPECT_EQ( + traj_controller_->get_node()->deactivate().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + } } static void TearDownTestCase() { rclcpp::shutdown(); } - void subscribeToState() + void subscribeToStateLegacy() + { + auto traj_lifecycle_node = traj_controller_->get_node(); + traj_lifecycle_node->set_parameter( + rclcpp::Parameter("state_publish_rate", static_cast(100))); + + using control_msgs::msg::JointTrajectoryControllerState; + + auto qos = rclcpp::SensorDataQoS(); + // Needed, otherwise spin_some() returns only the oldest message in the queue + // I do not understand why spin_some provides only one message + qos.keep_last(1); + state_legacy_subscriber_ = + traj_lifecycle_node->create_subscription( + controller_name_ + "/state", qos, + [&](std::shared_ptr msg) + { + std::lock_guard guard(state_legacy_mutex_); + state_legacy_msg_ = msg; + }); + } + + void subscribeToState(rclcpp::Executor & executor) { auto traj_lifecycle_node = traj_controller_->get_node(); traj_lifecycle_node->set_parameter( @@ -278,23 +423,33 @@ class TrajectoryControllerTest : public ::testing::Test // I do not understand why spin_some provides only one message qos.keep_last(1); state_subscriber_ = traj_lifecycle_node->create_subscription( - controller_name_ + "/state", qos, + controller_name_ + "/controller_state", qos, [&](std::shared_ptr msg) { std::lock_guard guard(state_mutex_); state_msg_ = msg; }); + + const auto timeout = std::chrono::milliseconds{10}; + const auto until = traj_lifecycle_node->get_clock()->now() + timeout; + while (traj_lifecycle_node->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } } /// Publish trajectory msgs with multiple points /** * delay_btwn_points - delay between each points - * points - vector of trajectories. One point per controlled joint - * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided points + * points_positions - vector of trajectory-positions. One point per controlled joint + * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided + * points + * points - vector of trajectory-velocities. One point per controlled joint */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, - const std::vector> & points, rclcpp::Time start_time, + const std::vector> & points_positions, rclcpp::Time start_time, const std::vector & joint_names = {}, const std::vector> & points_velocities = {}) { @@ -314,14 +469,17 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_msgs::msg::JointTrajectory traj_msg; if (joint_names.empty()) { - traj_msg.joint_names = {joint_names_.begin(), joint_names_.begin() + points[0].size()}; + traj_msg.joint_names = { + joint_names_.begin(), + joint_names_.begin() + + static_cast::difference_type>(points_positions[0].size())}; } else { traj_msg.joint_names = joint_names; } traj_msg.header.stamp = start_time; - traj_msg.points.resize(points.size()); + traj_msg.points.resize(points_positions.size()); builtin_interfaces::msg::Duration duration_msg; duration_msg.sec = delay_btwn_points.sec; @@ -329,14 +487,14 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Duration duration(duration_msg); rclcpp::Duration duration_total(duration_msg); - for (size_t index = 0; index < points.size(); ++index) + for (size_t index = 0; index < points_positions.size(); ++index) { traj_msg.points[index].time_from_start = duration_total; - traj_msg.points[index].positions.resize(points[index].size()); - for (size_t j = 0; j < points[index].size(); ++j) + traj_msg.points[index].positions.resize(points_positions[index].size()); + for (size_t j = 0; j < points_positions[index].size(); ++j) { - traj_msg.points[index].positions[j] = points[index][j]; + traj_msg.points[index].positions[j] = points_positions[index][j]; } duration_total = duration_total + duration; } @@ -353,39 +511,85 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_publisher_->publish(traj_msg); } - void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; - while (clock.now() < end_time) + auto previous_time = start_time; + + while (clock.now() <= end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + auto now = clock.now(); + traj_controller_->update(now, now - previous_time); + previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); } } - void waitAndCompareState( + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); + } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + return end_time; + } + + rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, - rclcpp::Duration controller_wait_time, double allowed_delta) + rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { { std::lock_guard guard(state_mutex_); state_msg_.reset(); } traj_controller_->wait_for_trajectory(executor); - updateController(controller_wait_time); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + auto end_time = updateControllerAsync(controller_wait_time, start_time); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta); } } @@ -395,11 +599,18 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } + std::shared_ptr getStateLegacy() const + { + std::lock_guard guard(state_legacy_mutex_); + return state_legacy_msg_; + } std::shared_ptr getState() const { std::lock_guard guard(state_mutex_); @@ -407,6 +618,145 @@ class TrajectoryControllerTest : public ::testing::Test } void test_state_publish_rate_target(int target_msg_count); + void expectCommandPoint( + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) + { + // it should be holding the given point + // i.e., active but trivial trajectory (one point only) + EXPECT_TRUE(traj_controller_->has_trivial_traj()); + + if (traj_controller_->use_closed_loop_pid_adapter() == false) + { + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(velocity.at(0), joint_vel_[0]); + EXPECT_EQ(velocity.at(1), joint_vel_[1]); + EXPECT_EQ(velocity.at(2), joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + else // traj_controller_->use_closed_loop_pid_adapter() == true + { + // velocity or effort PID? + // --> set kp > 0.0 in test + if (traj_controller_->has_velocity_command_interface()) + { + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } + } + if (traj_controller_->has_effort_command_interface()) + { + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } + } + } + } + + void expectHoldingPointDeactivated(std::vector point) + { + // it should be holding the given point, but no active trajectory + EXPECT_FALSE(traj_controller_->has_active_traj()); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_joints( + std::vector state_joint_names, std::vector command_joint_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_joint_names.size() * state_interface_types_.size()); + for (const auto & joint : state_joint_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_joint_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), + command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + std::string controller_name_; std::vector joint_names_; @@ -422,6 +772,10 @@ class TrajectoryControllerTest : public ::testing::Test state_subscriber_; mutable std::mutex state_mutex_; std::shared_ptr state_msg_; + rclcpp::Subscription::SharedPtr + state_legacy_subscriber_; + mutable std::mutex state_legacy_mutex_; + std::shared_ptr state_legacy_msg_; std::vector joint_pos_; std::vector joint_vel_; @@ -453,6 +807,8 @@ class TrajectoryControllerTestParameterized state_interface_types_ = std::get<1>(GetParam()); } + virtual void TearDown() { TrajectoryControllerTest::TearDown(); } + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } }; diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst new file mode 100644 index 0000000000..d72b94b920 --- /dev/null +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mecanum_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- +* Add changelog for mecanum_drive_controller +* [mecanum_drive_controller] Fix Odometry Initialization (backport `#1573 `_) (`#1583 `_) +* Add Mecanum Drive Controller (backport `#512 `_, `#1444 `_, `#1547 `_) (`#1376 `_) +* Contributors: Christoph Froehlich, Julia Jia, mergify[bot] diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..aa9c1e1411 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.16) +project(mecanum_drive_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(mecanum_drive_controller_parameters + src/mecanum_drive_controller.yaml +) + +add_library( + mecanum_drive_controller + SHARED + src/mecanum_drive_controller.cpp + src/odometry.cpp +) +target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) +target_include_directories(mecanum_drive_controller PUBLIC + "$" + "$") +target_link_libraries(mecanum_drive_controller PUBLIC + mecanum_drive_controller_parameters) +ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface mecanum_drive_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) + ament_target_dependencies(test_load_mecanum_drive_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) + target_include_directories(test_mecanum_drive_controller PRIVATE include) + target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) + target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/mecanum_drive_controller +) + +install( + TARGETS mecanum_drive_controller mecanum_drive_controller_parameters + EXPORT export_mecanum_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..eea18ba232 --- /dev/null +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -0,0 +1,76 @@ +.. _mecanum_drive_controller_userdoc: + +mecanum_drive_controller +========================= + +Library with shared functionalities for mobile robot controllers with mecanum drive (four mecanum wheels). +The library implements generic odometry and update methods and defines the main interfaces. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. +In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + + - odometry publishing as Odometry and TF message; + - input command timeout based on a parameter. + +Note about odometry calculation: +In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. +We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves). + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +When controller is in chained mode, it exposes the following references which can be commanded by the preceding controller: + +- ``/linear/x/velocity``, in m/s +- ``/linear/y/velocity``, in m/s +- ``/angular/z/velocity``, in rad/s + +Commands +,,,,,,,,, +- ``<*_wheel_command_joint_name>/velocity``, in rad/s + +States +,,,,,,, +- ``/velocity``, in rad/s + +.. note:: + + ``joint_name`` can be of ``*_wheel_state_joint_name`` parameter (if used), ``*_wheel_command_joint_name`` otherwise. + + +Subscribers +,,,,,,,,,,,, +Used when the controller is not in chained mode (``in_chained_mode == false``). + +- ``/reference`` [``geometry_msgs/msg/TwistStamped``] + Velocity command for the controller, if ``use_stamped_vel == true``. + +- ``/reference_unstamped`` [``geometry_msgs/msg/Twist``] + Velocity command for the controller, if ``use_stamped_vel == false``. + +Publishers +,,,,,,,,,,, +- ``/odometry`` [``nav_msgs/msg/Odometry``] +- ``/tf_odometry`` [``tf2_msgs/msg/TFMessage``] +- ``/controller_state`` [``control_msgs/msg/MecanumDriveControllerState``] + +Parameters +,,,,,,,,,,, + + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: ../src/mecanum_drive_controller.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/mecanum_drive_controller_params.yaml + :language: yaml diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp new file mode 100644 index 0000000000..495a4354a0 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -0,0 +1,169 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ +#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "control_msgs/msg/mecanum_drive_controller_state.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +// auto-generated by generate_parameter_library +#include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp" +#include "mecanum_drive_controller/odometry.hpp" +#include "mecanum_drive_controller/visibility_control.h" +namespace mecanum_drive_controller +{ +// name constants for state interfaces +static constexpr size_t NR_STATE_ITFS = 4; + +// name constants for command interfaces +static constexpr size_t NR_CMD_ITFS = 4; + +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 3; + +class MecanumDriveController : public controller_interface::ChainableControllerInterface +{ +public: + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + MecanumDriveController(); + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers() override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerReferenceMsgUnstamped = geometry_msgs::msg::Twist; + using OdomStateMsg = nav_msgs::msg::Odometry; + using TfStateMsg = tf2_msgs::msg::TFMessage; + using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState; + +protected: + std::shared_ptr param_listener_; + mecanum_drive_controller::Params params_; + + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ + enum WheelIndex : std::size_t + { + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + REAR_RIGHT = 2, + REAR_LEFT = 3 + }; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + */ + std::vector command_joint_names_; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + * If parameters for state joint names are *not* defined, this list is the same as + * `command_joint_names_`. + */ + std::vector state_joint_names_; + + // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. + // used for preceding controller + std::vector reference_names_; + + // Command subscribers and Controller State, odom state, tf state publishers + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_unstamped_subscriber_ = + nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + using OdomStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + std::unique_ptr rt_odom_state_publisher_; + + using TfStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + Odometry odometry_; + + bool use_stamped_vel_ = true; + +private: + // callback for topic interface + MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); + MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL + void reference_unstamped_callback(const std::shared_ptr msg); + + double velocity_in_center_frame_linear_x_; // [m/s] + double velocity_in_center_frame_linear_y_; // [m/s] + double velocity_in_center_frame_angular_z_; // [rad/s] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp new file mode 100644 index 0000000000..79a1b54846 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include "geometry_msgs/msg/twist.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" + +#define PLANAR_POINT_DIM 3 + +namespace mecanum_drive_controller +{ +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef std::function IntegrationFunction; + + /// \brief Constructor + /// Timestamp will get the current time value + /// Value will be set to zero + Odometry(); + + /// \brief Initialize the odometry + /// \param time Current time + void init(const rclcpp::Time & time, std::array base_frame_offset); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel_front_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_right_vel Wheel velocity [rad/s] + /// \param wheel_front_right_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated + bool update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt); + + /// \return position (x component) [m] + double getX() const { return position_x_in_base_frame_; } + /// \return position (y component) [m] + double getY() const { return position_y_in_base_frame_; } + /// \return orientation (z component) [m] + double getRz() const { return orientation_z_in_base_frame_; } + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const { return velocity_in_base_frame_linear_x; } + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const { return velocity_in_base_frame_linear_y; } + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const + { + return velocity_in_base_frame_angular_z; + ; + } + + const std::array & getBaseFrameOffset() const + { + return base_frame_offset_; + } + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param + /// (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] + void setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); + +private: + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Reference frame (wrt to center frame). [x, y, theta] + std::array base_frame_offset_; + + /// Current pose: + double position_x_in_base_frame_; // [m] + double position_y_in_base_frame_; // [m] + double orientation_z_in_base_frame_; // [rad] + + double velocity_in_base_frame_linear_x; // [m/s] + double velocity_in_base_frame_linear_y; // [m/s] + double velocity_in_base_frame_angular_z; // [rad/s] + + /// Wheels kinematic parameters [m]: + /// lx and ly represent the distance from the robot's center to the wheels + /// projected on the x and y axis with origin at robots center respectively, + /// sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly + double sum_of_robot_center_projection_on_X_Y_axis_; + double wheels_radius_; // [m] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h new file mode 100644 index 0000000000..3222b2fa52 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef MECANUM_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml new file mode 100644 index 0000000000..c012ad53ca --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -0,0 +1,7 @@ + + + + The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 0000000000..0be26bfac2 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,50 @@ + + + + mecanum_drive_controller + 2.44.0 + Implementation of mecanum drive controller for 4 wheel drive. + + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + Giridhar Bukka + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp new file mode 100644 index 0000000000..cdda1b46b4 --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -0,0 +1,538 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using ControllerReferenceMsgUnstamped = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsgUnstamped; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace mecanum_drive_controller +{ +MecanumDriveController::MecanumDriveController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn MecanumDriveController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + auto prepare_lists_with_joint_names = + [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( + const std::size_t index, const std::string & command_joint_name, + const std::string & state_joint_name) + { + command_joints[index] = command_joint_name; + if (state_joint_name.empty()) + { + state_joints[index] = command_joint_name; + } + else + { + state_joints[index] = state_joint_name; + } + }; + + command_joint_names_.resize(4); + state_joint_names_.resize(4); + + // The joint names are sorted according to the order documented in the header file! + prepare_lists_with_joint_names( + FRONT_LEFT, params_.front_left_wheel_command_joint_name, + params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + FRONT_RIGHT, params_.front_right_wheel_command_joint_name, + params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_RIGHT, params_.rear_right_wheel_command_joint_name, + params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_LEFT, params_.rear_left_wheel_command_joint_name, + params_.rear_left_wheel_state_joint_name); + + // Initialize odometry + std::array base_frame_offset = { + {params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, + params_.kinematics.base_frame_offset.theta}}; + odometry_.init(get_node()->now(), base_frame_offset); + + // Set wheel params for the odometry computation + odometry_.setWheelsParams( + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, + params_.kinematics.wheels_radius); + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + use_stamped_vel_ = params_.use_stamped_vel; + if (use_stamped_vel_) + { + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); + } + else + { + ref_unstamped_subscriber_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &MecanumDriveController::reference_unstamped_callback, this, std::placeholders::_1)); + } + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = + get_node()->create_publisher("~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = + get_node()->create_publisher("~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // controller State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, + "Exception thrown during publisher creation at configure stage " + "with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); + + return controller_interface::CallbackReturn::SUCCESS; +} + +void MecanumDriveController::reference_callback(const std::shared_ptr msg) +{ + // If no timestamp provided, use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + // Check the timeout condition + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older by %.10f than allowed timeout (%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + + reset_controller_reference_msg(msg, get_node()); + } +} + +void MecanumDriveController::reference_unstamped_callback( + const std::shared_ptr msg) +{ + // Write fake header in the stored stamped command + auto twist_stamped = *(input_ref_.readFromNonRT()); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + input_ref_.writeFromNonRT(twist_stamped); +} + +controller_interface::InterfaceConfiguration +MecanumDriveController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_joint_names_.size()); + for (const auto & joint : command_joint_names_) + { + command_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration MecanumDriveController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joint_names_.size()); + + for (const auto & joint : state_joint_names_) + { + state_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return state_interfaces_config; +} + +std::vector +MecanumDriveController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +bool MecanumDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn MecanumDriveController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (size_t i = 0; i < NR_CMD_ITFS; ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MecanumDriveController::update_reference_from_subscribers() +{ + // Move functionality to the `update_and_write_commands` because of the missing arguments in + // humble - otherwise issues with multiple time-sources might happen when working with simulators + return controller_interface::return_type::OK; +} + +controller_interface::return_type MecanumDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // FORWARD KINEMATICS (odometry). + const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); + const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); + const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); + + if ( + !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && + !std::isnan(wheel_rear_right_state_vel) && !std::isnan(wheel_front_right_state_vel)) + { + // Estimate twist (using joint information) and integrate + odometry_.update( + wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, + wheel_front_right_state_vel, period.seconds()); + } + + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.linear.y; + reference_interfaces_[2] = current_ref->twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + } + else + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + + // INVERSE KINEMATICS (move robot). + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic `~/reference`) is a body twist. + if ( + !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && + !std::isnan(reference_interfaces_[2])) + { + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); + /// \note The variables meaning: + /// rotation_from_base_to_center: Rotation transformation matrix, to transform from + /// base frame to center frame + /// linear_trans_from_base_to_center: offset/linear transformation matrix, to + /// transform from base frame to center frame + + tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); + tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = + rotation_from_base_to_center * + tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( + params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); + + velocity_in_center_frame_linear_x_ = + velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y_ = + velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * reference_interfaces_[2]; + velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; + + const double wheel_front_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_front_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + + // Set wheels velocities - The joint names are sorted according to the order documented in the + // header file! + command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel); + command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel); + command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel); + command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); + } + else + { + command_interfaces_[FRONT_LEFT].set_value(0.0); + command_interfaces_[FRONT_RIGHT].set_value(0.0); + command_interfaces_[REAR_RIGHT].set_value(0.0); + command_interfaces_[REAR_LEFT].set_value(0.0); + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getRz()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getVx(); + rt_odom_state_publisher_->msg_.twist.twist.linear.y = odometry_.getVy(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.front_left_wheel_velocity = + state_interfaces_[FRONT_LEFT].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = + state_interfaces_[FRONT_RIGHT].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = + state_interfaces_[REAR_RIGHT].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = + state_interfaces_[REAR_LEFT].get_value(); + controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace mecanum_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mecanum_drive_controller::MecanumDriveController, + controller_interface::ChainableControllerInterface) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml new file mode 100644 index 0000000000..3cbe51e1f2 --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -0,0 +1,153 @@ +mecanum_drive_controller: + reference_timeout: { + type: double, + default_value: 0.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + # Command joint names + front_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + front_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + # State joint names + front_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + front_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + kinematics: + base_frame_offset: + x: { + type: double, + default_value: 0.0, + description: "Base frame offset along X axis of base_frame (base_link frame)." , + read_only: true, + } + y: { + type: double, + default_value: 0.0, + description: "Base frame offset along Y axis of base_frame (base_link frame)." , + read_only: true, + } + theta: { + type: double, + default_value: 0.0, + description: "Base frame offset along Theta axis of base_frame (base_link frame).", + read_only: true, + } + + wheels_radius: { + type: double, + default_value: 0.0, + description: "Wheel's radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + sum_of_robot_center_projection_on_X_Y_axis: { + type: double, + default_value: 0.0, + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", + read_only: false, + } + + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled?", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of pose covariance matrix.", + read_only: false, + } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..bb873fcfdb --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mecanum_drive_controller/odometry.hpp" + +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace mecanum_drive_controller +{ +Odometry::Odometry() +: timestamp_(0.0), + position_x_in_base_frame_(0.0), + position_y_in_base_frame_(0.0), + orientation_z_in_base_frame_(0.0), + velocity_in_base_frame_linear_x(0.0), + velocity_in_base_frame_linear_y(0.0), + velocity_in_base_frame_angular_z(0.0), + sum_of_robot_center_projection_on_X_Y_axis_(0.0), + wheels_radius_(0.0) +{ +} + +void Odometry::init( + const rclcpp::Time & time, std::array base_frame_offset) +{ + // Reset timestamp: + timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; +} + +bool Odometry::update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt) +{ + /// We cannot estimate the speed with very small time intervals: + // const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) return false; // Interval too small to integrate with + + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity + /// at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and + /// let the user perform post-processing at will. + /// We prefer this way of doing as filtering introduces delay (which makes it difficult + /// to interpret and compare behavior curves). + + /// \note The variables meaning: + /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform + /// from center frame to base frame + /// linear_transformation_from_center_2_base: offset/linear transformation matrix, + /// to transform from center frame to base frame + + double velocity_in_center_frame_linear_x = + 0.25 * wheels_radius_ * + (wheel_front_left_vel + wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_linear_y = + 0.25 * wheels_radius_ * + (-wheel_front_left_vel + wheel_rear_left_vel - wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_angular_z = + 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * + (-wheel_front_left_vel - wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + + tf2::Quaternion orientation_R_c_b; + orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); + + tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); + tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ = + angular_transformation_from_center_2_base * + tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); + tf2::Vector3 linear_transformation_from_center_2_base = + angular_transformation_from_center_2_base * + tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + velocity_in_base_frame_linear_x = + velocity_in_center_frame_w_r_t_base_frame_.x() + + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_linear_y = + velocity_in_center_frame_w_r_t_base_frame_.y() - + linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; + + /// Integration. + /// NOTE: the position is expressed in the odometry frame , unlike the twist which is + /// expressed in the body frame. + orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; + + tf2::Quaternion orientation_R_b_odom; + orientation_R_b_odom.setRPY(0.0, 0.0, orientation_z_in_base_frame_); + + tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); + tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = + angular_transformation_from_base_2_odom * + tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); + + position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; + position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; + + return true; +} + +void Odometry::setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius) +{ + sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; + wheels_radius_ = wheels_radius; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml new file mode 100644 index 0000000000..c764c6c063 --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -0,0 +1,40 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + + +test_mecanum_drive_controller_with_rotation: + ros__parameters: + reference_timeout: 5.0 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "rear_right_wheel_joint" + rear_left_wheel_command_joint_name: "rear_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 1.0, y: 2.0, theta: 3.0 } + wheels_radius: 0.05 + sum_of_robot_center_projection_on_X_Y_axis: 0.2925 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml new file mode 100644 index 0000000000..c4e5bb391a --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -0,0 +1,24 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.1 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..f326fc49a1 --- /dev/null +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -0,0 +1,54 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMecanumDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/mecanum_drive_controller_params.yaml"; + + cm.set_parameter({"test_mecanum_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_mecanum_drive_controller.type", "mecanum_drive_controller/MecanumDriveController"}); + + ASSERT_NE(cm.load_controller("test_mecanum_drive_controller"), nullptr); + + rclcpp::shutdown(); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..c4bd712d3d --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -0,0 +1,656 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.0); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 0.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.9); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 1.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + // When state joint names are empty they are the same as the command joint names + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(command_joint_names_)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check command itfs configuration + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check state itfs configuration + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs configuration, + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + + std::string("/") + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), reference_interface_names[i]); + } +} + +TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + + ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +// when update is called expect the previously set reference before calling update, +// inside the controller state message +TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + controller_->reference_interfaces_[0] = 1.5; + + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); +} + +// reference_interfaces and command_interfaces values depend on the reference_msg, +// the below test shows two cases when reference_msg is not received and when it is received. +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + // no reference_msg sent + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); + ASSERT_EQ(msg.back_left_wheel_velocity, 0.1); + ASSERT_EQ(msg.back_right_wheel_velocity, 0.1); +} + +// when too old msg is sent expect reference msg reset +TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = *(controller_->input_ref_.readFromNonRT()); + auto old_timestamp = reference->header.stamp; + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); +} + +// when time stamp is zero expect that time stamp is set to current time stamp +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(rclcpp::Time(0)); + + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); +} + +// when the reference_msg has valid timestamp then the timeout check in reference_callback() +// shall pass and reference_msg is not reset +TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +// when not in chainable mode and ref_msg_timedout expect +// command_interfaces are set to 0.0 and when ref_msg is not timedout expect +// command_interfaces are set to valid command values +TEST_F( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) +{ + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // BACK Left vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * + // 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } +} + +// when in chained mode the reference_interfaces of chained controller and command_interfaces +// of preceding controller point to same memory location, hence reference_interfaces are not +// exclusively set by the update method of chained controller +TEST_F( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + // imitating preceding controllers command_interfaces setting reference_interfaces of chained + // controller. + controller_->reference_interfaces_[0] = 3.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 6.0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + } +} + +// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces +// are calculated to valid values and reference_interfaces are unset +TEST_F( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); +} + +TEST_F( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + // reference_callback() is called implicitly when publish_commands() is called. + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) +{ + // Initialize controller + SetUpController("test_mecanum_drive_controller_with_rotation"); + + // Configure + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check on the base frame offset is set by the params + EXPECT_EQ(controller_->odometry_.getBaseFrameOffset()[0], 1.0); + EXPECT_EQ(controller_->odometry_.getBaseFrameOffset()[1], 2.0); + EXPECT_EQ(controller_->odometry_.getBaseFrameOffset()[2], 3.0); + + // Activate in chained mode + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->is_in_chained_mode(), true); + + // Setup reference interfaces for side to side motion + auto side_to_side_motion = [this](double linear_y) + { + controller_->reference_interfaces_[0] = 0; // linear x + controller_->reference_interfaces_[1] = linear_y; // linear y + controller_->reference_interfaces_[2] = 0; // angular z + }; + + // Setup reference interfaces for rotation + auto rotation_motion = [this](double rotation_velocity) + { + controller_->reference_interfaces_[0] = 0; // linear x + controller_->reference_interfaces_[1] = 0; // linear y + controller_->reference_interfaces_[2] = rotation_velocity; // angular z + }; + + const double update_rate = 50.0; // 50 Hz + const double dt = 1.0 / update_rate; + const double test_duration = 1.0; // 1 second test + auto current_time = controller_->get_node()->now(); + + auto count = 0; + for (double t = 0; t < test_duration; t += dt) + { + switch (count % 4) + { + case 0: + side_to_side_motion(2.0); + break; + case 1: + rotation_motion(-0.5); + break; + case 2: + side_to_side_motion(-2.0); + break; + case 3: + rotation_motion(0.5); + } + + // Call update method + ASSERT_EQ( + controller_->update(current_time, rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + + current_time += rclcpp::Duration::from_seconds(dt); + count++; + + // Update the state of the wheels for subsequent loop + size_t fl_index = controller_->get_front_left_wheel_index(); + size_t fr_index = controller_->get_front_right_wheel_index(); + size_t rl_index = controller_->get_rear_left_wheel_index(); + size_t rr_index = controller_->get_rear_right_wheel_index(); + joint_state_values_[fl_index] = controller_->command_interfaces_[fl_index].get_value(); + joint_state_values_[fr_index] = controller_->command_interfaces_[fr_index].get_value(); + joint_state_values_[rl_index] = controller_->command_interfaces_[rl_index].get_value(); + joint_state_values_[rr_index] = controller_->command_interfaces_[rr_index].get_value(); + } + + RCLCPP_INFO( + controller_->get_node()->get_logger(), "odometry: %f, %f, %f", controller_->odometry_.getX(), + controller_->odometry_.getY(), controller_->odometry_.getRz()); + + // Verify odometry remains bounded + EXPECT_LT(std::abs(controller_->odometry_.getX()), 1.0); + EXPECT_LT(std::abs(controller_->odometry_.getY()), 1.0); + EXPECT_LT(std::abs(controller_->odometry_.getRz()), M_PI); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp new file mode 100644 index 0000000000..ce500fbbbe --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -0,0 +1,316 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#define TEST_MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using TfStateMsg = mecanum_drive_controller::MecanumDriveController::TfStateMsg; +using OdomStateMsg = mecanum_drive_controller::MecanumDriveController::OdomStateMsg; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController +{ + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set); + FRIEND_TEST( + MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success); + FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); + FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); + FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); + FRIEND_TEST(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return mecanum_drive_controller::MecanumDriveController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + size_t get_front_left_wheel_index() { return WheelIndex::FRONT_LEFT; } + + size_t get_front_right_wheel_index() { return WheelIndex::FRONT_RIGHT; } + + size_t get_rear_right_wheel_index() { return WheelIndex::REAR_RIGHT; } + + size_t get_rear_left_wheel_index() { return WheelIndex::REAR_LEFT; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MecanumDriveControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/reference", rclcpp::SystemDefaultsQoS()); + + odom_s_publisher_node_ = std::make_shared("odom_s_publisher"); + odom_s_publisher_ = odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/odometry", rclcpp::SystemDefaultsQoS()); + + tf_odom_s_publisher_node_ = std::make_shared("tf_odom_s_publisher"); + tf_odom_s_publisher_ = tf_odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + command_joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface( + command_joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_mecanum_drive_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + // call update to publish the test value + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands( + const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, + const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.header.stamp = stamp; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = twist_linear_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = twist_angular_z; + + command_publisher_->publish(msg); + } + +protected: + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_CMD_JOINT_NAME[] = "back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_CMD_JOINT_NAME[] = "back_left_wheel_joint"; + std::vector command_joint_names_ = { + TEST_FRONT_LEFT_CMD_JOINT_NAME, TEST_FRONT_RIGHT_CMD_JOINT_NAME, TEST_REAR_RIGHT_CMD_JOINT_NAME, + TEST_REAR_LEFT_CMD_JOINT_NAME}; + + static constexpr char TEST_FRONT_LEFT_STATE_JOINT_NAME[] = "state_front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_STATE_JOINT_NAME[] = "state_front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_STATE_JOINT_NAME[] = "state_back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_STATE_JOINT_NAME[] = "state_back_left_wheel_joint"; + std::vector state_joint_names_ = { + TEST_FRONT_LEFT_STATE_JOINT_NAME, TEST_FRONT_RIGHT_STATE_JOINT_NAME, + TEST_REAR_RIGHT_STATE_JOINT_NAME, TEST_REAR_LEFT_STATE_JOINT_NAME}; + std::string interface_name_ = hardware_interface::HW_IF_VELOCITY; + + // Controller-related parameters + + std::array joint_state_values_ = {{0.1, 0.1, 0.1, 0.1}}; + std::array joint_command_values_ = {{101.101, 101.101, 101.101, 101.101}}; + + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; + double command_lin_x = 111; + + std::vector state_itfs_; + std::vector command_itfs_; + + double ref_timeout_ = 0.1; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Node::SharedPtr tf_odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; +}; + +#endif // TEST_MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp new file mode 100644 index 0000000000..edbee8a702 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ( + controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); +} + +// checking if all interfaces, command and state interfaces are exported as expected +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst new file mode 100644 index 0000000000..86bc54335f --- /dev/null +++ b/pid_controller/CHANGELOG.rst @@ -0,0 +1,227 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pid_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* [pid_controller] Update tests (backport `#1538 `_) (`#1545 `_) +* [pid_controller] Fix logic for feedforward_mode with single reference interface (backport `#1520 `_) (`#1539 `_) +* Improve antiwindup description (backport `#1502 `_) (`#1503 `_) +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich, Victor Coutinho Vieira Santos + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* fixes for windows compilation (`#1330 `_) (`#1332 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* 🚀 Add PID controller 🎉 (backport `#434 `_, `#975 `_, `#899 `_, `#1084 `_, `#951 `_) (`#1162 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000..937f1708ce --- /dev/null +++ b/pid_controller/CMakeLists.txt @@ -0,0 +1,131 @@ +cmake_minimum_required(VERSION 3.16) +project(pid_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +if(WIN32) + add_compile_definitions( + # For math constants + _USE_MATH_DEFINES + # Minimize Windows namespace collision + NOMINMAX + WIN32_LEAN_AND_MEAN + ) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pid_controller_parameters + src/pid_controller.yaml +) + +add_library(pid_controller SHARED + src/pid_controller.cpp +) +target_compile_features(pid_controller PUBLIC cxx_std_17) +target_include_directories(pid_controller PUBLIC + $ + $ +) +target_link_libraries(pid_controller PUBLIC + pid_controller_parameters +) +ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_pid_controller + test/test_pid_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller PRIVATE include) + target_link_libraries(test_pid_controller pid_controller) + ament_target_dependencies( + test_pid_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_pid_controller_preceding + test/test_pid_controller_preceding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml + ) + target_include_directories(test_pid_controller_preceding PRIVATE include) + target_link_libraries(test_pid_controller_preceding pid_controller) + ament_target_dependencies( + test_pid_controller_preceding + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_pid_controller_dual_interface + test/test_pid_controller_dual_interface.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller_dual_interface PRIVATE include) + target_link_libraries(test_pid_controller_dual_interface pid_controller) + ament_target_dependencies( + test_pid_controller_dual_interface + controller_interface + hardware_interface + ) + + ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) + target_include_directories(test_load_pid_controller PRIVATE include) + ament_target_dependencies( + test_load_pid_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/pid_controller +) + +install(TARGETS + pid_controller + pid_controller_parameters + EXPORT export_pid_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pid_controller/README.md b/pid_controller/README.md new file mode 100644 index 0000000000..01e8fddac8 --- /dev/null +++ b/pid_controller/README.md @@ -0,0 +1,7 @@ +pid_controller +========================================== + +Controller based on PID implementation from control_toolbox package. + +Pluginlib-Library: pid_controller +Plugin: pid_controller/PidController (controller_interface::ControllerInterface) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst new file mode 100644 index 0000000000..fc94357ab3 --- /dev/null +++ b/pid_controller/doc/userdoc.rst @@ -0,0 +1,103 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst + +.. _pid_controller_userdoc: + +PID Controller +-------------------------------- + +PID Controller implementation that uses PidROS implementation from `control_toolbox `_ package. +The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers. +It also enables to use the first derivative of the reference and its feedback to have second-order PID control. + +Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example: + +- reference/state POSITION; command VELOCITY --> PI CONTROLLER +- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER + +- reference/state VELOCITY; command POSITION --> PD CONTROLLER +- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER + +- reference/state POSITION; command POSITION --> PID CONTROLLER +- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER +- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER +- reference/state EFFORT; command EFFORT --> PID CONTROLLER + +.. note:: + + Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)` for the same purpose by sending only one reference point into it. + Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that. + PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware. + +Execution logic of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics. +If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type. +For example a valid combination would be ``position`` and ``velocity`` interface types. + +Using the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Pluginlib-Library: pid_controller +Plugin name: pid_controller/PidController + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + +Commands +,,,,,,,,, +- / [double] + +States +,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + + +Subscribers +,,,,,,,,,,,, +If controller is not in chained mode (``in_chained_mode == false``): + +- /reference [control_msgs/msg/MultiDOFCommand] + +If controller parameter ``use_external_measured_states`` is true: + +- /measured_state [control_msgs/msg/MultiDOFCommand] + +Services +,,,,,,,,,,, + +- /set_feedforward_control [std_srvs/srv/SetBool] + +Publishers +,,,,,,,,,,, +- /controller_state [control_msgs/msg/MultiDOFStateStamped] + +Parameters +,,,,,,,,,,, + +The PID controller uses the `generate_parameter_library `_ to handle its parameters. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pid_controller.yaml + + +An example parameter file +========================= + + +An example parameter file for this controller can be found in `the test folder (standalone) `_: + +.. literalinclude:: ../test/pid_controller_params.yaml + :language: yaml + +or as `preceding controller `_: + +.. literalinclude:: ../test/pid_controller_preceding_params.yaml + :language: yaml diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp new file mode 100644 index 0000000000..9dfa669537 --- /dev/null +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -0,0 +1,138 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER__PID_CONTROLLER_HPP_ + +#include +#include +#include + +#include "control_msgs/msg/multi_dof_command.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "control_toolbox/pid_ros.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "pid_controller/pid_controller_parameters.hpp" +#include "pid_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/joint_controller_state.hpp" + +#include "control_msgs/msg/pid_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace pid_controller +{ + +enum class feedforward_mode_type : std::uint8_t +{ + OFF = 0, + ON = 1, +}; + +class PidController : public controller_interface::ChainableControllerInterface +{ +public: + PID_CONTROLLER__VISIBILITY_PUBLIC + PidController(); + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; + using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; + +protected: + controller_interface::return_type update_reference_from_subscribers() override; + + std::shared_ptr param_listener_; + pid_controller::Params params_; + + std::vector reference_and_state_dof_names_; + size_t dof_; + std::vector measured_state_values_; + + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector feedforward_gain_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> measured_state_; + + rclcpp::Service::SharedPtr set_feedforward_control_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + // internal methods + void update_parameters(); + controller_interface::CallbackReturn configure_parameters(); + +private: + // callback for topic interface + PID_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace pid_controller + +#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_ diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h new file mode 100644 index 0000000000..1509364b5a --- /dev/null +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PID_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pid_controller/package.xml b/pid_controller/package.xml new file mode 100644 index 0000000000..82aa212db6 --- /dev/null +++ b/pid_controller/package.xml @@ -0,0 +1,46 @@ + + + + pid_controller + 2.44.0 + Controller based on PID implememenation from control_toolbox package. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + + ament_cmake + + generate_parameter_library + + angles + backward_ros + control_msgs + control_toolbox + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/pid_controller/pid_controller.xml b/pid_controller/pid_controller.xml new file mode 100644 index 0000000000..5b90741ae6 --- /dev/null +++ b/pid_controller/pid_controller.xml @@ -0,0 +1,8 @@ + + + + PidController ros2_control controller. + + + diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp new file mode 100644 index 0000000000..b181adf65f --- /dev/null +++ b/pid_controller/src/pid_controller.cpp @@ -0,0 +1,537 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "pid_controller/pid_controller.hpp" + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "control_msgs/msg/single_dof_state.hpp" +#include "controller_interface/helpers.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + msg->dof_names = dof_names; + msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); +} + +void reset_controller_measured_state_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + reset_controller_reference_msg(msg, dof_names); +} + +} // namespace + +namespace pid_controller +{ +PidController::PidController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PidController::on_init() +{ + control_mode_.initRT(feedforward_mode_type::OFF); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::update_parameters() +{ + if (!param_listener_->is_old(params_)) + { + return; + } + params_ = param_listener_->get_params(); +} + +controller_interface::CallbackReturn PidController::configure_parameters() +{ + update_parameters(); + + if (!params_.reference_and_state_dof_names.empty()) + { + reference_and_state_dof_names_ = params_.reference_and_state_dof_names; + } + else + { + reference_and_state_dof_names_ = params_.dof_names; + } + + if (params_.dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'dof_names' (%zu) and 'reference_and_state_dof_names' (%zu) parameters has to be " + "the same!", + params_.dof_names.size(), reference_and_state_dof_names_.size()); + return CallbackReturn::FAILURE; + } + + dof_ = params_.dof_names.size(); + + // TODO(destogl): is this even possible? Test it... + if (params_.gains.dof_names_map.size() != dof_) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'gains' (%zu) map and number or 'dof_names' (%zu) have to be the same!", + params_.gains.dof_names_map.size(), dof_); + return CallbackReturn::FAILURE; + } + + pids_.resize(dof_); + + for (size_t i = 0; i < dof_; ++i) + { + // prefix should be interpreted as parameters prefix + pids_[i] = + std::make_shared(get_node(), "gains." + params_.dof_names[i], true); + if (!pids_[i]->initPid()) + { + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reference_and_state_dof_names_.clear(); + pids_.clear(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = configure_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&PidController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + input_ref_.writeFromNonRT(msg); + + // input state Subscriber and callback + if (params_.use_external_measured_states) + { + auto measured_state_callback = + [&](const std::shared_ptr msg) -> void + { + // TODO(destogl): Sort the input values based on joint and interface names + measured_state_.writeFromNonRT(msg); + }; + measured_state_subscriber_ = get_node()->create_subscription( + "~/measured_state", subscribers_qos, measured_state_callback); + } + std::shared_ptr measured_state_msg = + std::make_shared(); + reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); + measured_state_.writeFromNonRT(measured_state_msg); + + measured_state_values_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + auto set_feedforward_control_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } + response->success = true; + }; + + set_feedforward_control_service_ = get_node()->create_service( + "~/set_feedforward_control", set_feedforward_control_callback, qos_services); + + try + { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Reserve memory in state publisher + state_publisher_->lock(); + state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) + { + state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + } + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::reference_callback(const std::shared_ptr msg) +{ + if (msg->dof_names.empty() && msg->values.size() == reference_and_state_dof_names_.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Reference massage does not have DoF names defined. " + "Assuming that value have order as defined state DoFs"); + auto ref_msg = msg; + ref_msg->dof_names = reference_and_state_dof_names_; + input_ref_.writeFromNonRT(ref_msg); + } + else if ( + msg->dof_names.size() == reference_and_state_dof_names_.size() && + msg->values.size() == reference_and_state_dof_names_.size()) + { + auto ref_msg = msg; // simple initialization + + // sort values in the ref_msg + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + + bool all_found = true; + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + auto found_it = + std::find(ref_msg->dof_names.begin(), ref_msg->dof_names.end(), msg->dof_names[i]); + if (found_it == msg->dof_names.end()) + { + all_found = false; + RCLCPP_WARN( + get_node()->get_logger(), "DoF name '%s' not found in the defined list of state DoFs.", + msg->dof_names[i].c_str()); + break; + } + + auto position = static_cast(std::distance(ref_msg->dof_names.begin(), found_it)); + ref_msg->values[position] = msg->values[i]; + ref_msg->values_dot[position] = msg->values_dot[i]; + } + + if (all_found) + { + input_ref_.writeFromNonRT(ref_msg); + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) and/or values (%zu) is not matching the expected size (%zu).", + msg->dof_names.size(), msg->values.size(), reference_and_state_dof_names_.size()); + } +} + +controller_interface::InterfaceConfiguration PidController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.dof_names.size()); + for (const auto & dof_name : params_.dof_names) + { + command_interfaces_config.names.push_back(dof_name + "/" + params_.command_interface); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (params_.use_external_measured_states) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces_config.names.push_back(dof_name + "/" + interface); + } + } + } + + return state_interfaces_config; +} + +std::vector PidController::on_export_reference_interfaces() +{ + reference_interfaces_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); + ++index; + } + } + + return reference_interfaces; +} + +bool PidController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn PidController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command (the same number as state interfaces) + reset_controller_reference_msg(*(input_ref_.readFromRT()), reference_and_state_dof_names_); + reset_controller_measured_state_msg( + *(measured_state_.readFromRT()), reference_and_state_dof_names_); + + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign( + measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PidController::update_reference_from_subscribers() +{ + auto current_ref = input_ref_.readFromRT(); + + for (size_t i = 0; i < dof_; ++i) + { + if (!std::isnan((*current_ref)->values[i])) + { + reference_interfaces_[i] = (*current_ref)->values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + { + reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + } + + (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + } + } + return controller_interface::return_type::OK; +} + +controller_interface::return_type PidController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // check for any parameter updates + update_parameters(); + + // Update feedback either from external measured state or from state interfaces + if (params_.use_external_measured_states) + { + const auto measured_state = *(measured_state_.readFromRT()); + for (size_t i = 0; i < dof_; ++i) + { + measured_state_values_[i] = measured_state->values[i]; + if (measured_state_values_.size() == 2 * dof_) + { + measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + } + } + } + else + { + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + measured_state_values_[i] = state_interfaces_[i].get_value(); + } + } + + for (size_t i = 0; i < dof_; ++i) + { + double tmp_command = 0.0; + + if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) + { + // calculate feed-forward + if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) + { + // two interfaces + if (reference_interfaces_.size() == 2 * dof_) + { + if (std::isfinite(reference_interfaces_[dof_ + i])) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + } + else // one interface + { + tmp_command = reference_interfaces_[i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + } + + double error = reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -picomputeCommand( + error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + } + else + { + // Fallback to calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + } + else + { + // use calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + + // write calculated values + command_interfaces_[i].set_value(tmp_command); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < dof_; ++i) + { + state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; + state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].error = + reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = + angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); + } + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].error_dot = + reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].time_step = period.seconds(); + // Command can store the old calculated values. This should be obvious because at least one + // another value is NaN. + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + } + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace pid_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + pid_controller::PidController, controller_interface::ChainableControllerInterface) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml new file mode 100644 index 0000000000..1727d2c8a9 --- /dev/null +++ b/pid_controller/src/pid_controller.yaml @@ -0,0 +1,90 @@ +pid_controller: + dof_names: { + type: string_array, + default_value: [], + description: "Specifies dof_names or axes used by the controller. If 'reference_and_state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + reference_and_state_dof_names: { + type: string_array, + default_value: [], + description: "(optional) Specifies dof_names or axes for getting reference and reading states. This parameter is only relevant when state dof names are different then command dof names, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + command_interface: { + type: string, + default_value: "", + description: "Name of the interface used by the controller for writing commands to the hardware.", + read_only: true, + validation: { + not_empty<>: null, + } + } + reference_and_state_interfaces: { + type: string_array, + default_value: [], + description: "Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivative of the first.", + read_only: true, + validation: { + not_empty<>: null, + size_lt<>: 3, + } + } + use_external_measured_states: { + type: bool, + default_value: false, + description: "Use external states from a topic instead from state interfaces." + } + gains: + __map_dof_names: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + antiwindup: { + type: bool, + default_value: false, + description: "Antiwindup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_clamp_max and + i_clamp_min are applied in both scenarios." + } + i_clamp_max: { + type: double, + default_value: 0.0, + description: "Upper integral clamp." + } + i_clamp_min: { + type: double, + default_value: 0.0, + description: "Lower integral clamp." + } + feedforward_gain: { + type: double, + default_value: 0.0, + description: "Gain for the feed-forward part." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (i.e., are continuous). + Normalizes position-error to -pi to pi." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml new file mode 100644 index 0000000000..0458c87012 --- /dev/null +++ b/pid_controller/test/pid_controller_params.yaml @@ -0,0 +1,49 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0} + +test_pid_controller_angle_wraparound_on: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, angle_wraparound: true} + +test_pid_controller_with_feedforward_gain: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} + +test_pid_controller_with_feedforward_gain_dual_interface: + ros__parameters: + dof_names: + - joint1 + - joint2 + + command_interface: velocity + + reference_and_state_interfaces: ["position", "velocity"] + + gains: + joint1: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} + joint2: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml new file mode 100644 index 0000000000..673abfe08e --- /dev/null +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + reference_and_state_dof_names: + - joint1state diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp new file mode 100644 index 0000000000..3a75f6e170 --- /dev/null +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); + + rclcpp::shutdown(); +} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp new file mode 100644 index 0000000000..0711d67a86 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,630 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 3.0); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); +} + +TEST_F(PidControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) + { + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); + } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +TEST_F(PidControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*msg)->values.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(PidControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_feedforward_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); +} + +/** + * @brief Check the update logic in non chained mode with feedforward OFF + * + */ + +TEST_F(PidControllerTest, test_update_logic_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + controller_->set_reference(dof_command_values_); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } + // check the command value + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 + const double expected_command_value = 30102.30102; + + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); +} + +/** + * @brief Check the update logic in non chained mode with feedforward ON and feedforward gain is 0 + * + */ + +TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + controller_->set_reference(dof_command_values_); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + + // check the command value: + // ref = 101.101, state = 1.1, ds = 0.01 + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward ON, feedforward_gain = 0 + // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3 + const double expected_command_value = 30102.301020; + + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + } +} + +/** + * @brief Check the update logic when chain mode is on. + * in chain mode, update_reference_from_subscribers is not called from update method, and the + * reference value is used for calculation + */ + +TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // set chain mode to true + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + // feedforward mode is off as default, use this for convenience + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // update reference interface which will be used for calculation + const double ref_interface_value = 5.0; + controller_->reference_interfaces_[0] = ref_interface_value; + + // publish a command message which should be ignored as chain mode is on + publish_commands({10.0}, {0.0}); + controller_->wait_for_commands(executor); + + // check the reference interface is not updated as chain mode is on + EXPECT_EQ(controller_->reference_interfaces_[0], ref_interface_value); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + + // check the command value + // ref = 5.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 + // error = ref - state = 5.0 - 1.1 = 3.9, error_dot = error/ds = 3.9/0.01 = 390.0, + // p_term = error * p_gain = 3.9 * 1.0 = 3.9, + // i_term = error * ds * i_gain = 3.9 * 0.01 * 2.0 = 0.078, + // d_term = error_dot * d_gain = 390.0 * 3.0 = 1170.0 + // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 + const double expected_command_value = 1173.978; + + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound); + + // write reference interface so that the values are would be wrapped + controller_->reference_interfaces_[0] = 10.0; + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the result of the commands - the values are not wrapped + const double expected_command_value = 2679.078; + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) +{ + SetUpController("test_pid_controller_angle_wraparound_on"); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Check on wraparound is on + ASSERT_TRUE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound); + + // Write reference interface with values that would wrap, state is 1.1 + controller_->reference_interfaces_[0] = 10.0; + + // Run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Check the command value + const double expected_command_value = 787.713559; + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); +} + +TEST_F(PidControllerTest, subscribe_and_get_messages_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + publish_commands(); + controller_->wait_for_commands(executor); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + } + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +/** + * @brief check chained pid controller with feedforward and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95 + const double target_value = 5.0; + const double expected_command_value = 6.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // send a message to update reference interface + controller_->set_reference({target_value}); + ASSERT_EQ( + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + +/** + * @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // given target value 5.0 + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward off, the command value should be still 1.95 even though feedforward gain + // is 1.0 + const double target_value = 5.0; + const double expected_command_value = 1.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // feedforward by default is OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // send a message to update reference interface + controller_->set_reference({target_value}); + ASSERT_EQ( + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp new file mode 100644 index 0000000000..d7086384dc --- /dev/null +++ b/pid_controller/test/test_pid_controller.hpp @@ -0,0 +1,301 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef TEST_PID_CONTROLLER_HPP_ +#define TEST_PID_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "pid_controller/pid_controller.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; +using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestablePidController : public pid_controller::PidController +{ + FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(PidControllerTest, activate_success); + FRIEND_TEST(PidControllerTest, reactivate_success); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update); + FRIEND_TEST(PidControllerTest, test_update_logic_angle_wraparound_off); + FRIEND_TEST(PidControllerTest, test_update_logic_angle_wraparound_on); + FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); + FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain); + FRIEND_TEST(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return pid_controller::PidController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return pid_controller::PidController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + void set_reference(const std::vector & target_value) + { + std::shared_ptr msg = std::make_shared(); + msg->dof_names = params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value[i]; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + input_ref_.writeFromNonRT(msg); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class PidControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + dof_names_ = {"joint1"}; + command_interface_ = "position"; + state_interfaces_ = {"position"}; + dof_state_values_ = {1.1}; + dof_command_values_ = {101.101}; + reference_and_state_dof_names_ = {"joint1state"}; + + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + feedforward_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_feedforward_control"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_pid_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", rclcpp::NodeOptions()), + controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(dof_names_.size()); + command_ifs.reserve(dof_names_.size()); + + for (size_t i = 0; i < dof_names_.size(); ++i) + { + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + dof_names_[i], command_interface_, &dof_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); + state_itfs_.reserve(dof_names_.size() * state_interfaces_.size()); + size_t index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); + state_ifs.emplace_back(state_itfs_.back()); + ++index; + } + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_pid_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands( + const std::vector & values = {0.45}, const std::vector & values_dot = {0.0}) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.dof_names = dof_names_; + msg.values = values; + msg.values_dot = values_dot; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool feedforward, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = feedforward; + + bool wait_for_service_ret = + feedforward_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Service is not available!"); + } + auto result = feedforward_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector dof_names_; + std::string command_interface_; + std::vector state_interfaces_; + std::vector dof_state_values_; + std::vector dof_command_values_; + std::vector reference_and_state_dof_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr feedforward_service_client_; +}; + +#endif // TEST_PID_CONTROLLER_HPP_ diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp new file mode 100644 index 0000000000..e006986473 --- /dev/null +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -0,0 +1,110 @@ +// 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. +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerDualInterfaceTest : public PidControllerFixture +{ +public: + void SetUp() + { + PidControllerFixture::SetUp(); + + dof_names_ = {"joint1", "joint2"}; + + // set up interfaces + command_interface_ = "velocity"; + state_interfaces_ = {"position", "velocity"}; + dof_state_values_ = { + get_joint1_state_position(), get_joint2_state_position(), get_joint1_state_velocity(), + get_joint2_state_velocity()}; + } + + double get_joint1_state_position() const { return 10.0; } + double get_joint2_state_position() const { return 11.0; } + double get_joint1_state_velocity() const { return 5.0; } + double get_joint2_state_velocity() const { return 6.0; } + + double get_joint1_reference_position() const { return 15.0; } + double get_joint2_reference_position() const { return 16.0; } + double get_joint1_reference_velocity() const { return 6.0; } + double get_joint2_reference_velocity() const { return 7.0; } +}; + +/** + * @brief Test the feedforward gain with chained mode with two interfaces + * The second interface should be derivative of the first one + */ + +TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface) +{ + SetUpController("test_pid_controller_with_feedforward_gain_dual_interface"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 0.3); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 0.4); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set up the reference interface, + controller_->reference_interfaces_ = { + get_joint1_reference_position(), get_joint2_reference_position(), + get_joint1_reference_velocity(), get_joint2_reference_velocity()}; + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the commands + const double joint1_expected_cmd = 8.915; + const double joint2_expected_cmd = 9.915; + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp new file mode 100644 index 0000000000..3e17e69286 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_EQ(controller_->params_.command_interface, command_interface_); +} + +TEST_F(PidControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) + { + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); + } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..9e61ea73d7 --- /dev/null +++ b/pose_broadcaster/CHANGELOG.rst @@ -0,0 +1,217 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pose_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* [pose_broadcaster] Check for valid pose before attempting to publish a tf for it (backport `#1479 `_) (`#1483 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Implement new PoseBroadcaster controller (backport `#1311 `_) (`#1326 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..46028cf258 --- /dev/null +++ b/pose_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.16) +project(pose_broadcaster + LANGUAGES + CXX +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + +add_library(pose_broadcaster SHARED + src/pose_broadcaster.cpp +) +target_compile_features(pose_broadcaster PUBLIC + cxx_std_17 +) +target_include_directories(pose_broadcaster PUBLIC + $ + $ +) +target_link_libraries(pose_broadcaster PUBLIC + pose_broadcaster_parameters +) +ament_target_dependencies(pose_broadcaster PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pose_broadcaster.xml +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_pose_broadcaster + test/test_load_pose_broadcaster.cpp + ) + target_link_libraries(test_load_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_load_pose_broadcaster + controller_manager + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_pose_broadcaster + test/test_pose_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml + ) + target_link_libraries(test_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_pose_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY + include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS + pose_broadcaster + pose_broadcaster_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pose_broadcaster/README.md b/pose_broadcaster/README.md new file mode 100644 index 0000000000..bf47411048 --- /dev/null +++ b/pose_broadcaster/README.md @@ -0,0 +1,8 @@ +pose_broadcaster +========================================== + +Controller to publish poses provided by pose sensors. + +Pluginlib-Library: pose_broadcaster + +Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..0ae40e2fad --- /dev/null +++ b/pose_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst + +.. _pose_broadcaster_userdoc: + +Pose Broadcaster +-------------------------------- +Broadcaster for poses measured by a robot or a sensor. +Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms. + +The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/pose_broadcaster_params.yaml + :language: yaml diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp new file mode 100644 index 0000000000..5923cb8890 --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -0,0 +1,78 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "pose_broadcaster/visibility_control.h" +#include "rclcpp/publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "semantic_components/pose_sensor.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include "pose_broadcaster/pose_broadcaster_parameters.hpp" + +namespace pose_broadcaster +{ + +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr pose_sensor_; + + rclcpp::Publisher::SharedPtr pose_publisher_; + std::unique_ptr> + realtime_publisher_; + + std::optional tf_publish_period_; + rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Publisher::SharedPtr tf_publisher_; + std::unique_ptr> + realtime_tf_publisher_; +}; + +} // namespace pose_broadcaster + +#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_ diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h new file mode 100644 index 0000000000..5ce272658d --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define POSE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define POSE_BROADCASTER_EXPORT __declspec(dllexport) +#define POSE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef POSE_BROADCASTER_BUILDING_DLL +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT +#else +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#else +#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define POSE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml new file mode 100644 index 0000000000..93d3efffc5 --- /dev/null +++ b/pose_broadcaster/package.xml @@ -0,0 +1,41 @@ + + + + pose_broadcaster + 2.44.0 + Broadcaster to publish cartesian states. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Robert Wilbrandt + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml new file mode 100644 index 0000000000..6578958004 --- /dev/null +++ b/pose_broadcaster/pose_broadcaster.xml @@ -0,0 +1,9 @@ + + + + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform. + + + diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp new file mode 100644 index 0000000000..535a12f419 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -0,0 +1,217 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "pose_broadcaster/pose_broadcaster.hpp" +#include +#include + +namespace +{ + +constexpr auto DEFAULT_POSE_TOPIC = "~/pose"; +constexpr auto DEFAULT_TF_TOPIC = "/tf"; + +} // namespace + +namespace pose_broadcaster +{ + +bool is_pose_valid(const geometry_msgs::msg::Pose & pose) +{ + return std::isfinite(pose.position.x) && std::isfinite(pose.position.y) && + std::isfinite(pose.position.z) && std::isfinite(pose.orientation.x) && + std::isfinite(pose.orientation.y) && std::isfinite(pose.orientation.z) && + std::isfinite(pose.orientation.w) && + + std::abs( + pose.orientation.x * pose.orientation.x + pose.orientation.y * pose.orientation.y + + pose.orientation.z * pose.orientation.z + pose.orientation.w * pose.orientation.w - + 1.0) <= 10e-3; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = pose_sensor_->get_state_interface_names(); + + return state_interfaces_config; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & ex) + { + fprintf(stderr, "Exception thrown during init stage with message: %s\n", ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + pose_sensor_ = std::make_unique(params_.pose_name); + tf_publish_period_ = + params_.tf.publish_rate == 0.0 + ? std::nullopt + : std::optional{rclcpp::Duration::from_seconds(1.0 / params_.tf.publish_rate)}; + + try + { + pose_publisher_ = get_node()->create_publisher( + DEFAULT_POSE_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>( + pose_publisher_); + + if (params_.tf.enable) + { + tf_publisher_ = get_node()->create_publisher( + DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_tf_publisher_ = + std::make_unique>( + tf_publisher_); + } + } + catch (const std::exception & ex) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s\n", + ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize pose message + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->unlock(); + + // Initialize tf message if tf publishing is enabled + if (realtime_tf_publisher_) + { + realtime_tf_publisher_->lock(); + + realtime_tf_publisher_->msg_.transforms.resize(1); + auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_transform.header.frame_id = params_.frame_id; + if (params_.tf.child_frame_id.empty()) + { + tf_transform.child_frame_id = params_.pose_name; + } + else + { + tf_transform.child_frame_id = params_.tf.child_frame_id; + } + + realtime_tf_publisher_->unlock(); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PoseBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + geometry_msgs::msg::Pose pose; + pose_sensor_->get_values_as_message(pose); + + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose = pose; + realtime_publisher_->unlockAndPublish(); + } + if (!is_pose_valid(pose)) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Invalid pose [%f, %f, %f], [%f, %f, %f, %f]", pose.position.x, pose.position.y, + pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, + pose.orientation.w); + } + else if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + { + bool do_publish = false; + // rlcpp::Time comparisons throw if clock types are not the same + if (tf_last_publish_time_.get_clock_type() != time.get_clock_type()) + { + do_publish = true; + } + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ <= time)) + { + do_publish = true; + } + + if (do_publish) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; + + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; + + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; + + realtime_tf_publisher_->unlockAndPublish(); + + tf_last_publish_time_ = time; + } + else + { + realtime_tf_publisher_->unlock(); + } + } + + return controller_interface::return_type::OK; +} + +} // namespace pose_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(pose_broadcaster::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml new file mode 100644 index 0000000000..11c53b5e57 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,32 @@ +pose_broadcaster: + frame_id: + type: string + default_value: "" + description: "frame_id in which values are published" + validation: + not_empty<>: null + pose_name: + type: string + default_value: "" + description: "Base name used as prefix for controller interfaces. + The state interface names are: ``/position.x, ..., /position.z, + /orientation.x, ..., /orientation.w``" + validation: + not_empty<>: null + tf: + enable: + type: bool + default_value: true + description: "Whether to publish the pose as a tf transform" + child_frame_id: + type: string + default_value: "" + description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left + empty." + publish_rate: + type: double + default_value: 0.0 + description: "Rate to limit publishing of tf transforms to (Hz). If set to 0, no limiting is + performed." + validation: + gt_eq<>: 0.0 diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml new file mode 100644 index 0000000000..a2f8477dd1 --- /dev/null +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_pose_broadcaster: + ros__parameters: + pose_name: "test_pose" + frame_id: "pose_frame" diff --git a/pose_broadcaster/test/test_load_pose_broadcaster.cpp b/pose_broadcaster/test/test_load_pose_broadcaster.cpp new file mode 100644 index 0000000000..a003bf7793 --- /dev/null +++ b/pose_broadcaster/test/test_load_pose_broadcaster.cpp @@ -0,0 +1,52 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPoseBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + const std::string test_file_path = + std::string{TEST_FILES_DIRECTORY} + "/pose_broadcaster_params.yaml"; + cm.set_parameter({"test_pose_broadcaster.params_file", test_file_path}); + + cm.set_parameter({"test_pose_broadcaster.type", "pose_broadcaster/PoseBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_pose_broadcaster"), nullptr); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp new file mode 100644 index 0000000000..5b300b23b3 --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -0,0 +1,253 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "test_pose_broadcaster.hpp" + +#include +#include +#include +#include + +using hardware_interface::LoanedStateInterface; + +void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } + +void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } + +void PoseBroadcasterTest::SetUpPoseBroadcaster() +{ + ASSERT_EQ( + pose_broadcaster_->init("test_pose_broadcaster"), controller_interface::return_type::OK); + + std::vector state_interfaces; + state_interfaces.emplace_back(pose_position_x_); + state_interfaces.emplace_back(pose_position_y_); + state_interfaces.emplace_back(pose_position_z_); + state_interfaces.emplace_back(pose_orientation_x_); + state_interfaces.emplace_back(pose_orientation_y_); + state_interfaces.emplace_back(pose_orientation_z_); + state_interfaces.emplace_back(pose_orientation_w_); + + pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); +} + +TEST_F(PoseBroadcasterTest, Configure_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command interface configuration + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ(command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + // Verify state interface configuration + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); +} + +TEST_F(PoseBroadcasterTest, Activate_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); + } + + // Deactivate controller + ASSERT_EQ( + pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); // Should not change when deactivating + } +} + +TEST_F(PoseBroadcasterTest, Update_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PoseBroadcasterTest, PublishSuccess) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_EQ(pose_msg.pose.position.x, pose_values_[0]); + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + subscribe_and_get_message("/tf", tf_msg); + + // Verify content of tf message + ASSERT_EQ(tf_msg.transforms.size(), 1lu); + EXPECT_EQ(tf_msg.transforms[0].header.frame_id, frame_id_); + EXPECT_EQ(tf_msg.transforms[0].child_frame_id, tf_child_frame_id_); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.x, pose_values_[0]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.y, pose_values_[1]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.z, pose_values_[2]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.x, pose_values_[3]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.y, pose_values_[4]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.z, pose_values_[5]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); +} + +TEST_F(PoseBroadcasterTest, invalid_pose_no_tf_published) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + pose_values_[0] = std::numeric_limits::quiet_NaN(); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_TRUE(std::isnan(pose_msg.pose.position.x)); // We set that to NaN above + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); + // Verify that no tf message was sent + ASSERT_EQ(tf_msg.transforms.size(), 0lu); + + // Set valid position + pose_values_[0] = 0.0; + // but invalid quaternion + pose_values_[3] = 0.0; + pose_values_[4] = 0.0; + pose_values_[5] = 0.0; + pose_values_[6] = 0.0; + + EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); + // Verify that no tf message was sent + ASSERT_EQ(tf_msg.transforms.size(), 0lu); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp new file mode 100644 index 0000000000..e3b84c0307 --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -0,0 +1,100 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef TEST_POSE_BROADCASTER_HPP_ +#define TEST_POSE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp/executors.hpp" + +#include "pose_broadcaster/pose_broadcaster.hpp" + +using pose_broadcaster::PoseBroadcaster; + +class PoseBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + + void SetUpPoseBroadcaster(); + +protected: + const std::string pose_name_ = "test_pose"; + const std::string frame_id_ = "pose_base_frame"; + const std::string tf_child_frame_id_ = "pose_frame"; + + std::array pose_values_ = { + {1.1, 2.2, 3.3, 0.39190382, 0.20056212, 0.53197575, 0.72331744}}; + + hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; + hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; + hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; + hardware_interface::StateInterface pose_orientation_x_{ + pose_name_, "orientation.x", &pose_values_[3]}; + hardware_interface::StateInterface pose_orientation_y_{ + pose_name_, "orientation.y", &pose_values_[4]}; + hardware_interface::StateInterface pose_orientation_z_{ + pose_name_, "orientation.z", &pose_values_[5]}; + hardware_interface::StateInterface pose_orientation_w_{ + pose_name_, "orientation.w", &pose_values_[6]}; + + std::unique_ptr pose_broadcaster_; + + template + void subscribe_and_get_message(const std::string & topic, T & msg); +}; + +template +void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T & msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + typename T::SharedPtr received_msg; + const auto msg_callback = [&](const typename T::SharedPtr sub_msg) { received_msg = sub_msg; }; + const auto subscription = node.create_subscription(topic, 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + if (i >= max_sub_check_loop_count) + { + throw std::runtime_error("Failed to receive message on topic: " + topic); + } + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + msg = *received_msg; +} + +#endif // TEST_POSE_BROADCASTER_HPP_ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 115fac4e37..9dab58b49a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,140 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- +* [ForwardCommandController] Fix the duplicate command interface types when reconfiguring the controller (backport `#1568 `_, `#1570 `_) (`#1569 `_) +* Contributors: mergify[bot] + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Update position controller package.xml (backport `#1431 `_) (`#1432 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index f1f57c6180..54a4f94656 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -1,85 +1,73 @@ -cmake_minimum_required(VERSION 3.5) -project(position_controllers) +cmake_minimum_required(VERSION 3.16) +project(position_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(position_controllers SHARED src/joint_group_position_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(position_controllers PUBLIC cxx_std_17) +target_include_directories(position_controllers PUBLIC + $ + $ ) +ament_target_dependencies(position_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(position_controllers PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_position_controller + ament_add_gmock(test_load_joint_group_position_controller test/test_load_joint_group_position_controller.cpp ) - target_include_directories(test_load_joint_group_position_controller PRIVATE include) + target_link_libraries(test_load_joint_group_position_controller + position_controllers + ) ament_target_dependencies(test_load_joint_group_position_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_position_controller + ament_add_gmock(test_joint_group_position_controller test/test_joint_group_position_controller.cpp ) - target_include_directories(test_joint_group_position_controller PRIVATE include) target_link_libraries(test_joint_group_position_controller - ${PROJECT_NAME} + position_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/position_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS position_controllers + EXPORT export_position_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_position_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst index 1e5dd53e63..89766df9d4 100644 --- a/position_controllers/doc/userdoc.rst +++ b/position_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/position_controllers/doc/userdoc.rst + .. _position_controllers_userdoc: position_controllers @@ -5,7 +7,41 @@ position_controllers This is a collection of controllers that work using the "position" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the position on a certain joint to achieve a set velocity. -Hardware interface type ------------------------ +The package contains the following controllers: + +position_controllers/JointGroupPositionController +------------------------------------------------- + +This is specialization of the :ref:`forward_command_controller ` that works using the "position" joint interface. + + +ROS 2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Topics +,,,,,,,,,,,,,,,,,, + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Joints' position commands + + +Parameters +,,,,,,,,,,,,,,,,,, +This controller overrides the interface parameter from :ref:`forward_command_controller `, and the +``joints`` parameter is the only one that is required. + +An example parameter file is given here + +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + position_controller: + type: position_controllers/JointGroupPositionController -These controllers work with joints using the "position" command interface. + position_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 21cdd59b86..0bdd4e9593 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,22 +1,33 @@ position_controllers - 2.15.0 - Generic controller for forwarding commands. + 2.44.0 + Generic position controller for forwarding position commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake + backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a712cc81c2..0996e04089 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -62,6 +51,7 @@ void JointGroupPositionControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) @@ -99,12 +89,13 @@ TEST_F(JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails) // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); - // activate failed, 'acceleration' is not a registered interface for `joint1` + // activate should succeed now ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) @@ -203,10 +194,13 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..c7d704db52 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -24,6 +24,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -58,6 +59,7 @@ class JointGroupPositionControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index cc376bd2d4..bc27b5e629 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupPositionController, load_controller) @@ -32,8 +34,10 @@ TEST(TestLoadJointGroupPositionController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_group_position_controller", "position_controllers/JointGroupPositionController")); + ASSERT_NE( + cm.load_controller( + "test_joint_group_position_controller", "position_controllers/JointGroupPositionController"), + nullptr); rclcpp::shutdown(); } diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..c675faaaaa --- /dev/null +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -0,0 +1,224 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package range_sensor_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* Let sphinx add parameter description with nested structures to documentation (backport `#652 `_) (`#1005 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* add a broadcaster for range sensor (backport `#725 `_) (`#766 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..00da395db5 --- /dev/null +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.8) +project(range_sensor_broadcaster) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +generate_parameter_library(range_sensor_broadcaster_parameters + src/range_sensor_broadcaster_parameters.yaml +) + +add_library( + range_sensor_broadcaster SHARED + src/range_sensor_broadcaster.cpp +) +target_include_directories(range_sensor_broadcaster + PRIVATE $ + PRIVATE $ +) +ament_target_dependencies(range_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(range_sensor_broadcaster PRIVATE "RANGE_SENSOR_BROADCASTER_BUILDING_DLL") +target_link_libraries(range_sensor_broadcaster + range_sensor_broadcaster_parameters +) + +pluginlib_export_plugin_description_file( + controller_interface range_sensor_broadcaster.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_range_sensor_broadcaster + test/test_load_range_sensor_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + target_include_directories(test_load_range_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_range_sensor_broadcaster + range_sensor_broadcaster + ) + ament_target_dependencies(test_load_range_sensor_broadcaster + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_range_sensor_broadcaster + test/test_range_sensor_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + target_include_directories(test_range_sensor_broadcaster PRIVATE include) + target_link_libraries(test_range_sensor_broadcaster + range_sensor_broadcaster + ) + ament_target_dependencies(test_range_sensor_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + range_sensor_broadcaster + range_sensor_broadcaster_parameters + EXPORT export_range_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_range_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/range_sensor_broadcaster/README.md b/range_sensor_broadcaster/README.md new file mode 100644 index 0000000000..33195d272b --- /dev/null +++ b/range_sensor_broadcaster/README.md @@ -0,0 +1,8 @@ +range_sensor_broadcaster +========================================== + +Controller to publish readings of Range sensors. + +Pluginlib-Library: range_sensor_broadcaster + +Plugin: range_sensor_broadcaster/RangeSensorBroadcaster (controller_interface::ControllerInterface) diff --git a/range_sensor_broadcaster/doc/userdoc.rst b/range_sensor_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..39fb98b3aa --- /dev/null +++ b/range_sensor_broadcaster/doc/userdoc.rst @@ -0,0 +1,31 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/range_sensor_broadcaster/doc/userdoc.rst + +.. _range_sensor_broadcaster_userdoc: + +Range Sensor Broadcaster +-------------------------------- +Broadcaster of messages from Range sensor. +The published message type is ``sensor_msgs/msg/Range``. + +The controller is a wrapper around ``RangeSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +The Range Sensor Broadcaster uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/range_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/range_sensor_broadcaster_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/range_sensor_broadcaster_params.yaml + :language: yaml diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp new file mode 100644 index 0000000000..a265595333 --- /dev/null +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -0,0 +1,78 @@ +// Copyright 2021 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: Subhas Das, Denis Stogl, Victor Lopez + */ + +#ifndef RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ +#define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "range_sensor_broadcaster/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "semantic_components/range_sensor.hpp" +#include "sensor_msgs/msg/range.hpp" + +#include "range_sensor_broadcaster/range_sensor_broadcaster_parameters.hpp" + +namespace range_sensor_broadcaster +{ +class RangeSensorBroadcaster : public controller_interface::ControllerInterface +{ +public: + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr range_sensor_; + + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; + std::unique_ptr realtime_publisher_; +}; + +} // namespace range_sensor_broadcaster + +#endif // RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h b/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h new file mode 100644 index 0000000000..0a9a9f53a8 --- /dev/null +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Subhas Das, Denis Stogl + */ + +#ifndef RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ +#define RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((dllexport)) +#define RANGE_SENSOR_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define RANGE_SENSOR_BROADCASTER_EXPORT __declspec(dllexport) +#define RANGE_SENSOR_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef RANGE_SENSOR_BROADCASTER_BUILDING_DLL +#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_EXPORT +#else +#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_IMPORT +#endif +#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE RANGE_SENSOR_BROADCASTER_PUBLIC +#define RANGE_SENSOR_BROADCASTER_LOCAL +#else +#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define RANGE_SENSOR_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define RANGE_SENSOR_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define RANGE_SENSOR_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define RANGE_SENSOR_BROADCASTER_PUBLIC +#define RANGE_SENSOR_BROADCASTER_LOCAL +#endif +#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml new file mode 100644 index 0000000000..ed53701928 --- /dev/null +++ b/range_sensor_broadcaster/package.xml @@ -0,0 +1,41 @@ + + + + range_sensor_broadcaster + 2.44.0 + Controller to publish readings of range sensors. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Florent Chretien + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/range_sensor_broadcaster/range_sensor_broadcaster.xml b/range_sensor_broadcaster/range_sensor_broadcaster.xml new file mode 100644 index 0000000000..fd82c7ae25 --- /dev/null +++ b/range_sensor_broadcaster/range_sensor_broadcaster.xml @@ -0,0 +1,8 @@ + + + + This controller publishes the readings of an Range sensor as sensor_msgs/Range message. + + + diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..72fdac3a5e --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -0,0 +1,137 @@ +// Copyright 2021 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: Subhas Das, Denis Stogl, Victor Lopez + */ + +#include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" + +#include +#include + +namespace range_sensor_broadcaster +{ +controller_interface::CallbackReturn RangeSensorBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + if (params_.sensor_name.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); + return CallbackReturn::ERROR; + } + + if (params_.frame_id.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); + return CallbackReturn::ERROR; + } + + range_sensor_ = std::make_unique( + semantic_components::RangeSensor(params_.sensor_name)); + try + { + // register ft sensor data publisher + sensor_state_publisher_ = + get_node()->create_publisher("~/range", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } + + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->msg_.radiation_type = params_.radiation_type; + realtime_publisher_->msg_.field_of_view = params_.field_of_view; + realtime_publisher_->msg_.min_range = params_.min_range; + realtime_publisher_->msg_.max_range = params_.max_range; + realtime_publisher_->unlock(); + + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +RangeSensorBroadcaster::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration RangeSensorBroadcaster::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = range_sensor_->get_state_interface_names(); + return state_interfaces_config; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + range_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + range_sensor_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type RangeSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + range_sensor_->get_values_as_message(realtime_publisher_->msg_); + realtime_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace range_sensor_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + range_sensor_broadcaster::RangeSensorBroadcaster, controller_interface::ControllerInterface) diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..c869cc27f2 --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml @@ -0,0 +1,15 @@ +range_sensor_broadcaster: + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } + radiation_type: {type: int, default_value: 0, description: "The type of radiation used by the sensor / 0 = Ultrason / 1 = Infrared",} + field_of_view: {type: double, default_value: 0.52, description: "The size of the arc that the distance reading is valid for [rad]",} + min_range: {type: double, default_value: 0.52, description: "Minimum range value [m]",} + max_range: {type: double, default_value: 4.0, description: "Maximum range value [m]",} diff --git a/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..7dae66a611 --- /dev/null +++ b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml @@ -0,0 +1,11 @@ +test_range_sensor_broadcaster: + ros__parameters: + # Setting mendatory parameters + sensor_name: "range_sensor" + frame_id: "range_sensor_frame" + + # Setting parameters with changed default value to check those are used + radiation_type: 1 + field_of_view: 0.1 + min_range: 0.10 + max_range: 7.0 diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..5c400bef91 --- /dev/null +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Subhas Das, Denis Stogl, Florent Chretien + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadRangeSensorBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_range_sensor_broadcaster", "range_sensor_broadcaster/RangeSensorBroadcaster"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..fb7c376224 --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -0,0 +1,284 @@ +// Copyright 2023 flochre +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: flochre + */ + +#include + +#include "test_range_sensor_broadcaster.hpp" + +#include "hardware_interface/loaned_state_interface.hpp" + +using testing::IsEmpty; +using testing::SizeIs; + +void RangeSensorBroadcasterTest::SetUp() +{ + // initialize controller + range_broadcaster_ = std::make_unique(); +} + +void RangeSensorBroadcasterTest::TearDown() { range_broadcaster_.reset(nullptr); } + +controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( + std::string broadcaster_name) +{ + controller_interface::return_type result = controller_interface::return_type::ERROR; + result = range_broadcaster_->init(broadcaster_name); + + if (controller_interface::return_type::OK == result) + { + std::vector state_interfaces; + state_interfaces.emplace_back(range_); + + range_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); + } + + return result; +} + +controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broadcaster( + std::vector & parameters) +{ + // Configure the broadcaster + for (auto parameter : parameters) + { + range_broadcaster_->get_node()->set_parameter(parameter); + } + + return range_broadcaster_->on_configure(rclcpp_lifecycle::State()); +} + +void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) +{ + // create a new subscriber + sensor_msgs::msg::Range::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr msg) { received_msg = msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_range_sensor_broadcaster/range", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + range_msg = *received_msg; +} + +TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) +{ + ASSERT_THROW(init_broadcaster(""), std::exception); +} + +TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Success) +{ + ASSERT_EQ( + init_broadcaster("test_range_sensor_broadcaster"), controller_interface::return_type::OK); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_1) +{ + // First Test without frame_id ERROR Expected + init_broadcaster("test_range_sensor_broadcaster"); + + std::vector parameters; + // explicitly give an empty sensor name to generate an error + parameters.emplace_back(rclcpp::Parameter("sensor_name", "")); + ASSERT_EQ(configure_broadcaster(parameters), controller_interface::CallbackReturn::ERROR); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_2) +{ + // Second Test without sensor_name ERROR Expected + init_broadcaster("test_range_sensor_broadcaster"); + + std::vector parameters; + // explicitly give an empty frame_id to generate an error + parameters.emplace_back(rclcpp::Parameter("frame_id", "")); + ASSERT_EQ(configure_broadcaster(parameters), controller_interface::CallbackReturn::ERROR); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success) +{ + // Third Test without sensor_name SUCCESS Expected + init_broadcaster("test_range_sensor_broadcaster"); + + ASSERT_EQ( + range_broadcaster_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); +} + +TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + + ASSERT_EQ( + range_broadcaster_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + ASSERT_EQ( + range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + range_broadcaster_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + auto result = range_broadcaster_->update( + range_broadcaster_->get_node()->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + + sensor_range_ = 0.10; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + + sensor_range_ = 4.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + + sensor_range_ = 0.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + // Even out of boundaries you will get the out_of_range range value + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + + sensor_range_ = 6.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + // Even out of boundaries you will get the out_of_range range value + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp new file mode 100644 index 0000000000..2b38cfa53c --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 flochre +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: flochre + */ + +#ifndef TEST_RANGE_SENSOR_BROADCASTER_HPP_ +#define TEST_RANGE_SENSOR_BROADCASTER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" + +class RangeSensorBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + +protected: + // for the sake of the test + // defining the parameter names same as in test/range_sensor_broadcaster_params.yaml + const std::string sensor_name_ = "range_sensor"; + const std::string frame_id_ = "range_sensor_frame"; + + const double field_of_view_ = 0.1; + const int radiation_type_ = 1; + const double min_range_ = 0.1; + const double max_range_ = 7.0; + + double sensor_range_ = 3.1; + hardware_interface::StateInterface range_{sensor_name_, "range", &sensor_range_}; + + std::unique_ptr range_broadcaster_; + + controller_interface::return_type init_broadcaster(std::string broadcaster_name); + controller_interface::CallbackReturn configure_broadcaster( + std::vector & parameters); + void subscribe_and_get_message(sensor_msgs::msg::Range & range_msg); +}; + +#endif // TEST_RANGE_SENSOR_BROADCASTER_HPP_ diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index b9cdead153..1b3910e7e7 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,7 +4,3 @@ repositories: # type: git # url: git@github.com:/.git # version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: humble diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.jazzy.repos similarity index 100% rename from ros2_controllers-not-released.foxy.repos rename to ros2_controllers-not-released.jazzy.repos diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 66352f4960..1b3910e7e7 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -1,5 +1,6 @@ repositories: - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 8c0ba1ff8a..61687f667e 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -2,15 +2,15 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master + version: humble realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: humble kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: master + version: humble control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git @@ -18,4 +18,4 @@ repositories: control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master + version: humble diff --git a/ros2_controllers.jazzy.repos b/ros2_controllers.jazzy.repos new file mode 100644 index 0000000000..e2a0c17bbc --- /dev/null +++ b/ros2_controllers.jazzy.repos @@ -0,0 +1,21 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: jazzy + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: jazzy + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: jazzy + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: jazzy diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index dfed17059b..cb7349c4ad 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -10,21 +10,12 @@ repositories: control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git - version: humble + version: master control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git version: ros2-master - kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0799e154c7..b43bc8c0d0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,138 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- +* Add Mecanum Drive Controller (backport `#512 `_, `#1444 `_, `#1547 `_) (`#1376 `_) + Co-authored-by: Dr. Denis + Co-authored-by: Christoph Froehlich + Co-authored-by: Shankar-Balajee + Co-authored-by: Soham Patil +* Contributors: mergify[bot] + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Add missing plugins to ros2_controllers dependencies (`#1413 `_) (`#1415 `_) +* Gpio command controller (backport `#1251 `_) (`#1372 `_) +* Contributors: Sai Kishor Kothakota, mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* 🚀 Add PID controller 🎉 (backport `#434 `_, `#975 `_, `#899 `_, `#1084 `_, `#951 `_) (`#1162 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) (`#1142 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* add a broadcaster for range sensor (backport `#725 `_) (`#766 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- +* Steering odometry library and controllers (backport `#484 `_) (`#624 `_) +* Contributors: Tomislav Petković, Reza Kermani, Denis Štogl + +2.21.0 (2023-05-28) +------------------- +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner) + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/CMakeLists.txt b/ros2_controllers/CMakeLists.txt index 8e3b75944d..bb7e2d25bb 100644 --- a/ros2_controllers/CMakeLists.txt +++ b/ros2_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.5) -project(ros2_controllers) +cmake_minimum_required(VERSION 3.16) +project(ros2_controllers LANGUAGES NONE) find_package(ament_cmake REQUIRED) ament_package() diff --git a/ros2_controllers/doc/conf.py b/ros2_controllers/doc/conf.py new file mode 100644 index 0000000000..b6134e9abd --- /dev/null +++ b/ros2_controllers/doc/conf.py @@ -0,0 +1,5 @@ +# Configuration file for the Sphinx documentation builder. +# settings will be overridden by rosdoc2, so we add here only custom settings + +copyright = "2024, ros2_control development team" +html_logo = "https://control.ros.org/master/_static/logo_ros-controls.png" diff --git a/ros2_controllers/doc/index.rst b/ros2_controllers/doc/index.rst new file mode 100644 index 0000000000..f785112944 --- /dev/null +++ b/ros2_controllers/doc/index.rst @@ -0,0 +1,80 @@ +Welcome to the documentation for ros2_controllers +================================================= + +For more information of the ros2_control framework see `control.ros.org `__. + +.. list-table:: + :header-rows: 1 + + * - Package Name + - Documentation + - API + - ROS Index + * - ackermann_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - admittance_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - bicycle_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - diff_drive_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - effort_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - force_torque_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - forward_command_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - imu_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - joint_state_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - joint_trajectory_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - pid_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - position_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - range_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - steering_controllers_library + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - tricycle_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - tricycle_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - velocity_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..578ef68957 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,25 +1,42 @@ ros2_controllers - 2.15.0 - Metapackage for ROS2 controllers related packages + 2.44.0 + Metapackage for ros2_controllers related packages + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + ament_cmake + ackermann_steering_controller admittance_controller + bicycle_steering_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster forward_command_controller + gpio_controllers + gripper_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + mecanum_drive_controller + pid_controller + pose_broadcaster position_controllers + range_sensor_broadcaster + steering_controllers_library tricycle_controller + tricycle_steering_controller velocity_controllers diff --git a/ros2_controllers/rosdoc2.yaml b/ros2_controllers/rosdoc2.yaml new file mode 100644 index 0000000000..c5ec1b53a0 --- /dev/null +++ b/ros2_controllers/rosdoc2.yaml @@ -0,0 +1,20 @@ +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + # Not generating any documentation of code + generate_package_index: false + always_run_doxygen: false + enable_breathe: false + enable_exhale: false + always_run_sphinx_apidoc: false + +builders: + # Configure Sphinx with the location of the docs: + - sphinx: { + name: 'ros2_controllers', + sphinx_sourcedir: 'doc', + output_dir: '' + } diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f60cb9d72a..63579da2f7 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,124 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- +* Don't call shutdown() after an exception (`#1400 `_) (`#1411 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Add another dependency (`#1382 `_) (`#1384 `_) +* Add missing deps for test_nodes (`#1378 `_) (`#1381 `_) +* test_nodes: catch keyboard interrupt and add simple launch tests (`#1369 `_) (`#1371 `_) +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 269d1da466..b7b439658d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,19 +2,28 @@ ros2_controllers_test_nodes - 2.15.0 + 2.44.0 Demo nodes for showing and testing functionalities of the ros2_control framework. - Denis Štogl Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 - Apache-2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ rclpy + sensor_msgs std_msgs trajectory_msgs python3-pytest + launch_testing_ros + launch_ros ament_python diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index 60ddd75854..7ea201386c 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -70,9 +70,12 @@ def main(args=None): publisher_forward_position = PublisherForwardPosition() - rclpy.spin(publisher_forward_position) - publisher_forward_position.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_forward_position) + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 56b56c5998..c2a38fe4d6 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -210,9 +210,12 @@ def main(args=None): publisher_joint_trajectory = PublisherJointTrajectory() - rclpy.spin(publisher_joint_trajectory) - publisher_joint_trajectory.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_joint_trajectory) + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ed3ae63656..85152fdb27 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,13 +20,12 @@ setup( name=package_name, - version="2.15.0", + version="2.44.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), - ("share/" + package_name, glob("launch/*.launch.py")), - ("share/" + package_name + "/configs", glob("configs/*.*")), + ("share/" + package_name + "/test", glob("test/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml new file mode 100644 index 0000000000..879ad34ab9 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0.0, 0.0] + pos3: [-0.785, -0.785] + pos4: [0.0, 0.0] diff --git a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml new file mode 100644 index 0000000000..7dd8304134 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_position_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_position_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - joint1 + - joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py new file mode 100644 index 0000000000..3f4c9da21f --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -0,0 +1,105 @@ +# Copyright (c) 2024 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 pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +import launch_testing.markers +from launch_testing_ros import WaitForTopics +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_forward_position_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# 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): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_forward_position_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_forward_position_controller not found!" + + def test_check_if_topic_published(self): + topic = "/forward_position_controller/commands" + wait_for_topics = WaitForTopics([(topic, Float64MultiArray)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.data) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py new file mode 100644 index 0000000000..62ad25550d --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -0,0 +1,105 @@ +# Copyright (c) 2024 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 pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +import launch_testing.markers +from launch_testing_ros import WaitForTopics +import rclpy +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_joint_trajectory_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# 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): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_position_trajectory_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_position_trajectory_controller not found!" + + def test_check_if_topic_published(self): + topic = "/joint_trajectory_position_controller/joint_trajectory" + wait_for_topics = WaitForTopics([(topic, JointTrajectory)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.joint_names) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros_controls.humble.repos b/ros_controls.humble.repos new file mode 100644 index 0000000000..79f01cbd00 --- /dev/null +++ b/ros_controls.humble.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: humble + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: humble diff --git a/ros_controls.jazzy.repos b/ros_controls.jazzy.repos new file mode 100644 index 0000000000..e7cfd385f8 --- /dev/null +++ b/ros_controls.jazzy.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: jazzy + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master diff --git a/ros_controls.rolling.repos b/ros_controls.rolling.repos new file mode 100644 index 0000000000..da64a510fd --- /dev/null +++ b/ros_controls.rolling.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: rolling + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d4b07df08b..2931cc960d 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,137 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- +* Update documentation of rqt_joint_trajectory_controller (backport `#1578 `_) (`#1582 `_) +* Contributors: Aditya Pawar + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- + +2.37.3 (2024-09-11) +------------------- +* Fix bug for displaying all controllers (`#1259 `_) (`#1271 `_) +* [RQT-JTC] limits from jtc controlled joints (`#1146 `_) (`#1274 `_) +* Contributors: mergify[bot] + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- +* [CI] Code coverage + pre-commit (backport `#1057 `_) (`#1064 `_) +* Contributors: mergify[bot] + +2.33.0 (2024-02-12) +------------------- + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- +* Add rqt_JTC to docs (`#950 `_) (`#952 `_) +* Contributors: mergify[bot] + +2.30.0 (2023-12-20) +------------------- +* Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) (`#897 `_) +* Adapted rqt_jtc to newest control_msgs for jtc (`#643 `_) (`#659 `_) +* Contributors: Guillaume Walck, Sai Kishor Kothakota + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- +* switch from dash to underscore in setup.cfg (`#595 `_) (`#599 `_) +* Contributors: mergify[bot] + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) (`#496 `_) +* Contributors: Denis Stogl + 2.15.0 (2022-12-06) ------------------- diff --git a/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png b/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png new file mode 100644 index 0000000000..cb86f4620b Binary files /dev/null and b/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png differ diff --git a/rqt_joint_trajectory_controller/doc/userdoc.rst b/rqt_joint_trajectory_controller/doc/userdoc.rst new file mode 100644 index 0000000000..335baf19dc --- /dev/null +++ b/rqt_joint_trajectory_controller/doc/userdoc.rst @@ -0,0 +1,21 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/rqt_joint_trajectory_controller/doc/userdoc.rst + +.. _rqt_joint_trajectory_controller_userdoc: + +rqt_joint_trajectory_controller +=============================== + +The rqt_joint_trajectory_controller provides an intuitive graphical way to test different joint positions and trajectories without having to manually construct complex trajectory messages or use command line interfaces. + +.. image:: rqt_joint_trajectory_controller.png + :width: 400 + :alt: rqt_joint_trajectory_controller + +The interface allows you to: + +* Select the controller manager namespace and controller from dropdown menus. +* Adjust target positions for joints (joint1 and joint2) using interactive sliders. +* Fine-tune joint positions with precise numerical inputs. +* Control the motion speed using the speed scaling slider. +* Activate the trajectory execution with the central power button. +* Visualize current joint configurations in real-time. diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 81e69f88fd..09867d9255 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,12 +4,19 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.15.0 + 2.44.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota - Apache-2.0 + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Adolfo Rodriguez Tsouroukdissian Noel Jimenez Garcia diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui index 3f3b6c1186..bc43f202e8 100644 --- a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -31,7 +31,7 @@ - <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namespace.</p></body></html> controller manager ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 7914a76b17..e77c4410dd 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -14,6 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +# Code inspired on the joint_state_publisher package by David Lu!!! +# https://github.com/ros/robot_model/blob/indigo-devel/ +# joint_state_publisher/joint_state_publisher/joint_state_publisher + # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got # Exception: Required attribute not set in XML: upper # upper is an optional attribute, so I don't understand what's going on @@ -33,21 +37,23 @@ def callback(msg): description = msg.data -def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True): - global description - use_small = use_smallest_joint_limits - use_mimic = True - - # Code inspired on the joint_state_publisher package by David Lu!!! - # https://github.com/ros/robot_model/blob/indigo-devel/ - # joint_state_publisher/joint_state_publisher/joint_state_publisher - +def subscribe_to_robot_description(node, key="robot_description"): qos_profile = rclpy.qos.QoSProfile(depth=1) qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE - node.create_subscription(String, "/robot_description", callback, qos_profile) - rclpy.spin_once(node) + node.create_subscription(String, key, callback, qos_profile) + + +def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): + use_small = use_smallest_joint_limits + use_mimic = True + + count = 0 + while description == "" and count < 10: + print("Waiting for the robot_description!") + count += 1 + rclpy.spin_once(node, timeout_sec=1.0) free_joints = {} dependent_joints = {} @@ -64,48 +70,56 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr if jtype == "fixed": continue name = child.getAttribute("name") - try: - limit = child.getElementsByTagName("limit")[0] - except IndexError: - continue - if jtype == "continuous": - minval = -pi - maxval = pi - else: + + if name in joints_names: try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: + limit = child.getElementsByTagName("limit")[0] + try: + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) + except ValueError: + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: + raise Exception( + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + except IndexError: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) + safety_tags = child.getElementsByTagName("safety_controller") + if use_small and len(safety_tags) == 1: + tag = safety_tags[0] + if tag.hasAttribute("soft_lower_limit"): + minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) + if tag.hasAttribute("soft_upper_limit"): + maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) + + mimic_tags = child.getElementsByTagName("mimic") + if use_mimic and len(mimic_tags) == 1: + tag = mimic_tags[0] + entry = {"parent": tag.getAttribute("joint")} + if tag.hasAttribute("multiplier"): + entry["factor"] = float(tag.getAttribute("multiplier")) + if tag.hasAttribute("offset"): + entry["offset"] = float(tag.getAttribute("offset")) + + dependent_joints[name] = entry continue - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - continue - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - if name in dependent_joints: - continue + if name in dependent_joints: + continue - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + joint = {"min_position": minval, "max_position": maxval} + joint["has_position_limits"] = jtype != "continuous" + joint["max_velocity"] = maxvel + free_joints[name] = joint return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 7e8ec4948e..5b27c2c832 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -29,7 +29,7 @@ from .utils import ControllerLister, ControllerManagerLister from .double_editor import DoubleEditor -from .joint_limits_urdf import get_joint_limits +from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description from .update_combo import update_combo # TODO: @@ -80,8 +80,8 @@ class JointTrajectoryController(Plugin): the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - - The controller exposes the C{command} and C{state} topics in its - ROS interface. + - The controller exposes the C{command} and C{controller_state} topics + in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. @@ -170,6 +170,9 @@ def __init__(self, context): self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() + # subscriptions + subscribe_to_robot_description(self._node) + # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) @@ -211,9 +214,9 @@ def restore_settings(self, plugin_settings, instance_settings): try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) - except (ValueError): + except ValueError: pass - except (ValueError): + except ValueError: pass # def trigger_configuration(self): @@ -235,7 +238,11 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation + self._robot_joint_limits = {} + for jtc_info in running_jtc: + self._robot_joint_limits.update( + get_joint_limits(self._node, _jtc_joint_names(jtc_info)) + ) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: @@ -252,10 +259,10 @@ def _update_jtc_list(self): def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() - def _on_joint_state_change(self, actual_pos): - assert len(actual_pos) == len(self._joint_pos) - for name in actual_pos.keys(): - self._joint_pos[name]["position"] = actual_pos[name] + def _on_joint_state_change(self, current_pos): + assert len(current_pos) == len(self._joint_pos) + for name in current_pos.keys(): + self._joint_pos[name]["position"] = current_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns @@ -289,11 +296,11 @@ def _on_jtc_enabled(self, val): self._speed_scaling_widget.setEnabled(val) if val: - # Widgets send desired position commands to controller + # Widgets send reference position commands to controller self._update_act_pos_timer.stop() self._update_cmd_timer.start() else: - # Controller updates widgets with actual position + # Controller updates widgets with feedback position self._update_cmd_timer.stop() self._update_act_pos_timer.start() @@ -332,7 +339,7 @@ def _load_jtc(self): # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) - state_topic = jtc_ns + "/state" + state_topic = jtc_ns + "/controller_state" cmd_topic = jtc_ns + "/joint_trajectory" self._state_sub = self._node.create_subscription( JointTrajectoryControllerState, state_topic, self._state_cb, 1 @@ -404,12 +411,12 @@ def _unregister_executor(self): self._executor = None def _state_cb(self, msg): - actual_pos = {} + current_pos = {} for i in range(len(msg.joint_names)): joint_name = msg.joint_names[i] - joint_pos = msg.actual.positions[i] - actual_pos[joint_name] = joint_pos - self.jointStateChanged.emit(actual_pos) + joint_pos = msg.feedback.positions[i] + current_pos[joint_name] = joint_pos + self.jointStateChanged.emit(current_pos) def _update_single_cmd_cb(self, val, name): self._joint_pos[name]["command"] = val @@ -424,7 +431,7 @@ def _update_cmd_cb(self): cmd = pos try: cmd = self._joint_pos[name]["command"] - except (KeyError): + except KeyError: pass max_vel = self._robot_joint_limits[name]["max_velocity"] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) @@ -442,7 +449,7 @@ def _update_joint_widgets(self): try: joint_pos = self._joint_pos[joint_name]["position"] joint_widgets[id].setValue(joint_pos) - except (KeyError): + except KeyError: pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? @@ -460,8 +467,9 @@ def _jtc_joint_names(jtc_info): joint_names = [] for interface in jtc_info.claimed_interfaces: - name = interface.split("/")[0] - joint_names.append(name) + name = interface.split("/")[-2] + if name not in joint_names: + joint_names.append(name) return joint_names diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index 4702d7fbc5..6f736910e2 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -36,7 +36,7 @@ def update_combo(combo, new_vals): selected_id = -1 try: selected_id = new_vals.index(selected_val) - except (ValueError): + except ValueError: combo.setCurrentIndex(-1) # Re-populate items @@ -48,7 +48,8 @@ def update_combo(combo, new_vals): def _is_permutation(a, b): - """Check is arrays are permutation of each other. + """ + Check is arrays are permutation of each other. @type a [] first array with values to compare with the second one. @type b [] second array with values to compare with the first one. diff --git a/rqt_joint_trajectory_controller/setup.cfg b/rqt_joint_trajectory_controller/setup.cfg index 4c0b982e25..59deadec31 100644 --- a/rqt_joint_trajectory_controller/setup.cfg +++ b/rqt_joint_trajectory_controller/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/rqt_joint_trajectory_controller +script_dir=$base/lib/rqt_joint_trajectory_controller [install] -install-scripts=$base/lib/rqt_joint_trajectory_controller +install_scripts=$base/lib/rqt_joint_trajectory_controller diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index afb93527f1..da83d41f9d 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -1,5 +1,19 @@ #!/usr/bin/env python +# Copyright 2024 Apache License, Version 2.0 +# +# 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 setuptools import setup from glob import glob @@ -7,7 +21,7 @@ setup( name=package_name, - version="2.15.0", + version="2.44.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst new file mode 100644 index 0000000000..3372af0e0f --- /dev/null +++ b/steering_controllers_library/CHANGELOG.rst @@ -0,0 +1,256 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package steering_controllers_library +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Fix open-loop odometry in case of ref timeout (backport `#1454 `_) (`#1460 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- +* Add Mecanum Drive Controller (backport `#512 `_, `#1444 `_, `#1547 `_) (`#1376 `_) +* Contributors: mergify[bot] + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Update generate_parameter_library dependency in steering_controllers_library (backport `#1465 `_) (`#1468 `_) +* Fix typos in steering_controllers_lib (backport `#1464 `_) (`#1466 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- +* fix(timeout): do not reset steer wheels to 0. on timeout (backport `#1289 `_) (`#1452 `_) +* steering_controllers_library: Add `reduce_wheel_speed_until_steering_reached` parameter (backport `#1314 `_) (`#1429 `_) +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* fix(steering-odometry): convert twist to steering angle (`#1288 `_) (`#1295 `_) +* fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) (`#1286 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Fix steering controllers library kinematics (`#1150 `_) (`#1194 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* [STEERING] Add missing `tan` call for ackermann (`#1117 `_) (`#1176 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) +* Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) (`#1153 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Fix usage of M_PI on Windows (`#1036 `_) (`#1037 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Update ci-ros-lint.yml and copyright format (backport `#720 `_) (`#918 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- +* Fix ackermann steering odometry (`#921 `_) (`#955 `_) +* Contributors: mergify[bot] + +2.30.0 (2023-12-20) +------------------- +* Changing default int values to double in steering controller's yaml file (`#927 `_) (`#928 `_) +* Contributors: mergify[bot] + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Steering controllers library: fix open loop mode (`#793 `_) (`#800 `_) +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- +* Bump versions for release +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* Second round of dependencies fix (`#655 `_) (`#656 `_) +* Fix sphinx for steering odometry library/controllers (`#626 `_) (`#661 `_) +* Remove unnecessary include (backport `#645 `_) (`#646 `_) +* Steering odometry library and controllers (backport `#484 `_) (`#624 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković, Reza Kermani, Denis Štogl + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt new file mode 100644 index 0000000000..741577ef5e --- /dev/null +++ b/steering_controllers_library/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.16) +project(steering_controllers_library LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(steering_controllers_library_parameters + src/steering_controllers_library.yaml +) + +add_library( + steering_controllers_library + SHARED + src/steering_controllers_library.cpp + src/steering_odometry.cpp +) +target_compile_features(steering_controllers_library PUBLIC cxx_std_17) +target_include_directories(steering_controllers_library PUBLIC + "$" + "$") +target_link_libraries(steering_controllers_library PUBLIC + steering_controllers_library_parameters) +ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL" "_USE_MATH_DEFINES") + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_steering_controllers_library test/test_steering_controllers_library.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_params.yaml) + target_include_directories(test_steering_controllers_library PRIVATE include) + target_link_libraries(test_steering_controllers_library steering_controllers_library) + ament_target_dependencies( + test_steering_controllers_library + controller_interface + hardware_interface + ) + ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) + target_link_libraries(test_steering_odometry steering_controllers_library) + +endif() + +install( + DIRECTORY include/ + DESTINATION include/steering_controllers_library +) + +install( + TARGETS steering_controllers_library steering_controllers_library_parameters + EXPORT export_steering_controllers_library + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_steering_controllers_library HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst new file mode 100644 index 0000000000..b051bb6420 --- /dev/null +++ b/steering_controllers_library/doc/userdoc.rst @@ -0,0 +1,118 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/steering_controllers_library/doc/userdoc.rst + +.. _steering_controllers_library_userdoc: + +steering_controllers_library +============================= + +.. _steering_controller_status_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerStatus.msg +.. _odometry_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/nav_msgs/msg/Odometry.msg +.. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg +.. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg + +Library with shared functionalities for mobile robot controllers with steering drives (2 degrees of freedom), with so-called non-holonomic constraints. + +The library implements generic odometry and update methods and defines the main interfaces. + +The update methods only use inverse kinematics, it does not implement any feedback control loops like path-tracking controllers etc. + +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped or unstamped `twist messages `_ where linear ``x`` and angular ``z`` components are used. +Values in other components are ignored. + +In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + +* support for front and rear steering configurations; +* odometry publishing as Odometry and TF message; +* input command timeout based on a parameter. + +The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. + +Currently implemented kinematics +-------------------------------------------------------------- + +* :ref:`Bicycle ` - with one steering and one drive joints; +* :ref:`Tricylce ` - with one steering and two drive joints; +* :ref:`Ackermann ` - with two steering and two drive joints. + +.. toctree:: + :hidden: + + Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> + Tricylce <../../tricycle_steering_controller/doc/userdoc.rst> + Ackermann <../../ackermann_steering_controller/doc/userdoc.rst> + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +Used when controller is in chained mode (``in_chained_mode == true``). + +- ``/linear/velocity`` double, in m/s +- ``/angular/velocity`` double, in rad/s + +representing the body twist. + +Command interfaces +,,,,,,,,,,,,,,,,,,, + +If parameter ``front_steering == true`` + +- ``/position`` double, in rad +- ``/velocity`` double, in m/s + +If parameter ``front_steering == false`` + +- ``/velocity`` double, in m/s +- ``/position`` double, in rad + +State interfaces +,,,,,,,,,,,,,,,,, + +Depending on the ``position_feedback``, different feedback types are expected + +* ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` +* ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` + +If parameter ``front_steering == true`` + +- ``/position`` double, in rad +- ``/`` double, in m or m/s + +If parameter ``front_steering == false`` + +- ``/`` double, in m or m/s +- ``/position`` double, in rad + +Subscribers +,,,,,,,,,,,, + +Used when controller is not in chained mode (``in_chained_mode == false``). + +- ``/reference`` [`geometry_msgs/msg/TwistStamped `_] + If parameter ``use_stamped_vel`` is ``true``. +- ``/reference_unstamped`` [geometry_msgs/msg/Twist] + If parameter ``use_stamped_vel`` is ``false``. + +Publishers +,,,,,,,,,,, + +- ``/odometry`` [`nav_msgs/msg/Odometry `_] +- ``/tf_odometry`` [`tf2_msgs/msg/TFMessage `_] +- ``/controller_state`` [`control_msgs/msg/SteeringControllerStatus `_] + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/steering_controllers_library.yaml diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp new file mode 100644 index 0000000000..77b8234b69 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -0,0 +1,151 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "steering_controllers_library/visibility_control.h" + +// TODO(anyone): Replace with controller specific messages +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "control_msgs/msg/steering_controller_status.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include "steering_controllers_library/steering_controllers_library_parameters.hpp" +#include "steering_controllers_library/steering_odometry.hpp" + +namespace steering_controllers_library +{ +class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface +{ +public: + STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary(); + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() = 0; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_reference_from_subscribers() override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; + using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerStateMsgOdom = nav_msgs::msg::Odometry; + using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; + using AckermannControllerState = control_msgs::msg::SteeringControllerStatus; + +protected: + controller_interface::CallbackReturn set_interface_numbers( + size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + + std::shared_ptr param_listener_; + steering_controllers_library::Params params_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + + using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; + using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + + std::unique_ptr rt_odom_state_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + /// Odometry: + steering_odometry::SteeringOdometry odometry_; + + AckermannControllerState published_state_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // name constants for state interfaces + size_t nr_state_itfs_; + // name constants for command interfaces + size_t nr_cmd_itfs_; + // name constants for reference interfaces + size_t nr_ref_itfs_; + + // last velocity commands for open loop odometry + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; + + std::vector rear_wheels_state_names_; + std::vector front_wheels_state_names_; + +private: + // callback for topic interface + STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( + const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); +}; + +} // namespace steering_controllers_library + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp new file mode 100644 index 0000000000..ad265751fa --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -0,0 +1,281 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ + +#include +#include +#include + +#include +#include "rcppmath/rolling_mean_accumulator.hpp" + +namespace steering_odometry +{ +const unsigned int BICYCLE_CONFIG = 0; +const unsigned int TRICYCLE_CONFIG = 1; +const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + +/** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ +class SteeringOdometry +{ +public: + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ + explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest velocity command + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + void update_open_loop(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Set odometry type + * \param type odometry type + */ + void set_odometry_type(const unsigned int type); + + /** + * \brief heading getter + * \return heading [rad] + */ + double get_heading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double get_x() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double get_y() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double get_linear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double get_angular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius, separation and wheelbase + */ + void set_wheel_params( + const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); + + /** + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle + * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * has been reached + * \return Tuple of velocity commands and steering commands + */ + std::tuple, std::vector> get_commands( + const double v_bx, const double omega_bz, const bool open_loop = true, + const bool reduce_wheel_speed_until_steering_reached = false); + + /** + * \brief Reset poses, heading, and accumulators + */ + void reset_odometry(); + +private: + /** + * \brief Uses precomputed linear and angular velocities to compute odometry + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + bool update_odometry(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Integrates the velocities (linear and angular) + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + void integrate_fk(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Calculates steering angle from the desired twist + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param omega_bz Angular velocity of the robot around x_z-axis + */ + double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); + + /** + * \brief Reset linear and angular accumulators + */ + void reset_accumulators(); + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double steer_pos_; // [rad] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Kinematic parameters + double wheel_track_; // [m] + double wheelbase_; // [m] + double wheel_radius_; // [m] + + /// Configuration type used for the forward kinematics + int config_type_ = -1; + + /// Previous wheel position/state [rad]: + double traction_wheel_old_pos_; + double traction_right_wheel_old_pos_; + double traction_left_wheel_old_pos_; + /// Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + rcppmath::RollingMeanAccumulator linear_acc_; + rcppmath::RollingMeanAccumulator angular_acc_; +}; +} // namespace steering_odometry + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h new file mode 100644 index 0000000000..123662031b --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -0,0 +1,50 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ +#define STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_EXPORT +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_IMPORT +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_PROTECTED __attribute__((visibility("protected"))) +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml new file mode 100644 index 0000000000..0c19f3df27 --- /dev/null +++ b/steering_controllers_library/package.xml @@ -0,0 +1,50 @@ + + + + steering_controllers_library + 2.44.0 + Package for steering robot configurations including odometry and interfaces. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + ament_cmake + + backward_ros + control_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp new file mode 100644 index 0000000000..4ff5d48df6 --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -0,0 +1,556 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers_library/steering_controllers_library.hpp" + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerTwistReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace steering_controllers_library +{ +SteeringControllersLibrary::SteeringControllersLibrary() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + initialize_implementation_parameter_listener(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::set_interface_numbers( + size_t nr_state_itfs = 2, size_t nr_cmd_itfs = 2, size_t nr_ref_itfs = 2) +{ + nr_state_itfs_ = nr_state_itfs; + nr_cmd_itfs_ = nr_cmd_itfs; + nr_ref_itfs_ = nr_ref_itfs; + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + odometry_.set_velocity_rolling_window_size( + static_cast(params_.velocity_rolling_window_size)); + + configure_odometry(); + + if (!params_.rear_wheels_state_names.empty()) + { + rear_wheels_state_names_ = params_.rear_wheels_state_names; + } + else + { + rear_wheels_state_names_ = params_.rear_wheels_names; + } + + if (!params_.front_wheels_state_names.empty()) + { + front_wheels_state_names_ = params_.front_wheels_state_names; + } + else + { + front_wheels_state_names_ = params_.front_wheels_names; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + if (params_.use_stamped_vel) + { + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); + } + else + { + ref_subscriber_unstamped_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); + } + + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void SteeringControllersLibrary::reference_callback( + const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +void SteeringControllersLibrary::reference_callback_unstamped( + const std::shared_ptr msg) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " + "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " + "future."); + auto twist_stamped = *(input_ref_.readFromNonRT()); + twist_stamped->header.stamp = get_node()->now(); + // if no timestamp provided use current time for command timestamp + if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + twist_stamped->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + twist_stamped->twist = *msg; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(nr_cmd_itfs_); + + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(nr_state_itfs_); + const auto traction_wheels_feedback = params_.position_feedback + ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + if (params_.front_steering) + { + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + + return state_interfaces_config; +} + +std::vector +SteeringControllersLibrary::on_export_reference_interfaces() +{ + reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs_); + + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + return reference_interfaces; +} + +bool SteeringControllersLibrary::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (size_t i = 0; i < nr_cmd_itfs_; ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers() +{ + // Move functionality to the `update_and_write_commands` because of the missing arguments in + // humble - otherwise issues with multiple time-sources might happen when working with simulators + + return controller_interface::return_type::OK; +} + +controller_interface::return_type SteeringControllersLibrary::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (!is_in_chained_mode()) + { + auto current_ref = *(input_ref_.readFromRT()); + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; + } + } + + update_odometry(period); + + // MOVE ROBOT + + // Limit velocities and accelerations: + // TODO(destogl): add limiter for the velocities + + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + { + const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + + // store (for open loop odometry) and set commands + last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; + last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; + + auto [traction_commands, steering_commands] = odometry_.get_commands( + reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, + params_.reduce_wheel_speed_until_steering_reached); + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + } + } + else + { + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); + } + } + } + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.get_heading()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odometry_.get_x(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odometry_.get_y(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.traction_wheels_position.clear(); + controller_state_publisher_->msg_.traction_wheels_velocity.clear(); + controller_state_publisher_->msg_.linear_velocity_command.clear(); + controller_state_publisher_->msg_.steer_positions.clear(); + controller_state_publisher_->msg_.steering_angle_command.clear(); + + auto number_of_traction_wheels = params_.rear_wheels_names.size(); + auto number_of_steering_wheels = params_.front_wheels_names.size(); + + if (!params_.front_steering) + { + number_of_traction_wheels = params_.front_wheels_names.size(); + number_of_steering_wheels = params_.rear_wheels_names.size(); + } + + for (size_t i = 0; i < number_of_traction_wheels; ++i) + { + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.push_back( + state_interfaces_[i].get_value()); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + state_interfaces_[i].get_value()); + } + controller_state_publisher_->msg_.linear_velocity_command.push_back( + command_interfaces_[i].get_value()); + } + + for (size_t i = 0; i < number_of_steering_wheels; ++i) + { + controller_state_publisher_->msg_.steer_positions.push_back( + state_interfaces_[number_of_traction_wheels + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.push_back( + command_interfaces_[number_of_traction_wheels + i].get_value()); + } + + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml new file mode 100644 index 0000000000..376642d4be --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -0,0 +1,129 @@ +steering_controllers_library: + reference_timeout: { + type: double, + default_value: 1.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + front_steering: { + type: bool, + default_value: true, + description: "Is the steering on the front of the robot?", + read_only: true, + } + + rear_wheels_names: { + type: string_array, + description: "Names of rear wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + front_wheels_names: { + type: string_array, + description: "Names of front wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + rear_wheels_state_names: { + type: string_array, + description: "(Optional) Names of rear wheel joints to read states from. If not set joint names from 'rear_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + front_wheels_state_names: { + type: string_array, + description: "(Optional) Names of front wheel joints to read states from. If not set joint names from 'front_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + open_loop: { + type: bool, + default_value: false, + description: "Choose if open-loop or not (feedback) is used for odometry calculation.", + read_only: false, + } + + reduce_wheel_speed_until_steering_reached: { + type: bool, + default_value: false, + description: "Reduce wheel speed until the steering angle has been reached.", + read_only: false, + } + + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", + read_only: false, + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + } + + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled?", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], + description: "diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], + description: "diagonal values of pose covariance matrix.", + read_only: false, + } + + position_feedback: { + type: bool, + default_value: false, + description: "Choice of feedback type, if position_feedback is false then ``HW_IF_VELOCITY`` is taken as interface type, if + position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", + read_only: false, + } + + use_stamped_vel: { + type: bool, + default_value: false, + description: "Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if + use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", + read_only: false, + } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp new file mode 100644 index 0000000000..3a97215ae7 --- /dev/null +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -0,0 +1,382 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#include "steering_controllers_library/steering_odometry.hpp" + +#include +#include +#include + +namespace steering_odometry +{ +SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_track_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + traction_wheel_old_pos_(0.0), + traction_right_wheel_old_pos_(0.0), + traction_left_wheel_old_pos_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_acc_(velocity_rolling_window_size), + angular_acc_(velocity_rolling_window_size) +{ +} + +void SteeringOdometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + reset_accumulators(); + timestamp_ = time; +} + +bool SteeringOdometry::update_odometry( + const double linear_velocity, const double angular_velocity, const double dt) +{ + /// Integrate odometry: + integrate_fk(linear_velocity, angular_velocity, dt); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_.accumulate(linear_velocity); + angular_acc_.accumulate(angular_velocity); + + linear_ = linear_acc_.getRollingMean(); + angular_ = angular_acc_.getRollingMean(); + + return true; +} + +bool SteeringOdometry::update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt) +{ + const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_; + + /// Update old position with current: + traction_wheel_old_pos_ = traction_wheel_pos; + + return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt); +} + +bool SteeringOdometry::update_from_position( + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double steer_pos, const double dt) +{ + const double traction_right_wheel_est_pos_diff = + traction_right_wheel_pos - traction_right_wheel_old_pos_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_pos - traction_left_wheel_old_pos_; + + /// Update old position with current: + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; + + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt); +} + +bool SteeringOdometry::update_from_position( + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt) +{ + const double traction_right_wheel_est_pos_diff = + traction_right_wheel_pos - traction_right_wheel_old_pos_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_pos - traction_left_wheel_old_pos_; + + /// Update old position with current: + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; + + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos, + left_steer_pos, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt) +{ + steer_pos_ = steer_pos; + double linear_velocity = traction_wheel_vel * wheel_radius_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular_velocity, dt); +} + +double SteeringOdometry::get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos) +{ + double turning_radius = wheelbase_ / std::tan(steer_pos); + const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; + const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; + + if (std::isinf(turning_radius)) + { + return (vel_wheel_r + vel_wheel_l) * 0.5; + } + + // overdetermined, we take the average + const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); + const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); + return (vel_r + vel_l) * 0.5; +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt) +{ + steer_pos_ = steer_pos; + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); + + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular_velocity, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt) +{ + // overdetermined, we take the average + const double right_steer_pos_est = std::atan( + wheelbase_ * std::tan(right_steer_pos) / + (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + const double left_steer_pos_est = std::atan( + wheelbase_ * std::tan(left_steer_pos) / + (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; + + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular_velocity, dt); +} + +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) +{ + /// Save last linear and angular velocity: + linear_ = v_bx; + angular_ = omega_bz; + + /// Integrate odometry: + integrate_fk(v_bx, omega_bz, dt); +} + +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) +{ + wheel_radius_ = wheel_radius; + wheelbase_ = wheelbase; + wheel_track_ = wheel_track; +} + +void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + reset_accumulators(); +} + +void SteeringOdometry::set_odometry_type(const unsigned int type) +{ + config_type_ = static_cast(type); +} + +double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) +{ + // phi can be nan if both v_bx and omega_bz are zero + const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + return std::isfinite(phi) ? phi : 0.0; +} + +std::tuple, std::vector> SteeringOdometry::get_commands( + const double v_bx, const double omega_bz, const bool open_loop, + const bool reduce_wheel_speed_until_steering_reached) +{ + // desired wheel speed and steering angle of the middle of traction and steering axis + double Ws, phi, phi_IK = steer_pos_; + +#if 0 + if (v_bx == 0 && omega_bz != 0) + { + // TODO(anyone) this would be only possible if traction is on the steering axis + phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; + } + else + { + // TODO(anyone) this would be valid only if traction is on the steering axis + Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle + } +#endif + // steering angle + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + if (open_loop) + { + phi_IK = phi; + } + // wheel speed + Ws = v_bx / wheel_radius_; + + if (!open_loop && reduce_wheel_speed_until_steering_reached) + { + // Reduce wheel speed until the target angle has been reached + double phi_delta = abs(steer_pos_ - phi); + double scale; + const double min_phi_delta = M_PI / 6.; + if (phi_delta < min_phi_delta) + { + scale = 1; + } + else if (phi_delta >= 1.5608) + { + // cos(1.5608) = 0.01 + scale = 0.01 / cos(min_phi_delta); + } + else + { + // TODO(anyone): find the best function, e.g convex power functions + scale = cos(phi_delta) / cos(min_phi_delta); + } + Ws *= scale; + } + + if (config_type_ == BICYCLE_CONFIG) + { + std::vector traction_commands = {Ws}; + std::vector steering_commands = {phi}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == TRICYCLE_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + // double-traction axle + if (is_close_to_zero(phi_IK)) + { + // avoid division by zero + traction_commands = {Ws, Ws}; + } + else + { + const double turning_radius = wheelbase_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + } + // simple steering + steering_commands = {phi}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == ACKERMANN_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + if (is_close_to_zero(phi_IK)) + { + // avoid division by zero + traction_commands = {Ws, Ws}; + // shortcut, no steering + steering_commands = {phi, phi}; + } + else + { + const double turning_radius = wheelbase_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + + const double numerator = 2 * wheelbase_ * std::sin(phi); + const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); + const double denominator_second_member = wheel_track_ * std::sin(phi); + + const double alpha_r = + std::atan2(numerator, denominator_first_member + denominator_second_member); + const double alpha_l = + std::atan2(numerator, denominator_first_member - denominator_second_member); + steering_commands = {alpha_r, alpha_l}; + } + return std::make_tuple(traction_commands, steering_commands); + } + else + { + throw std::runtime_error("Config not implemented"); + } +} + +void SteeringOdometry::reset_odometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + reset_accumulators(); +} + +void SteeringOdometry::integrate_runge_kutta_2( + const double v_bx, const double omega_bz, const double dt) +{ + // Compute intermediate value of the heading + const double theta_mid = heading_ + omega_bz * 0.5 * dt; + + // Use the intermediate values to update the state + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; + heading_ += omega_bz * dt; +} + +void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) +{ + const double delta_x_b = v_bx * dt; + const double delta_theta = omega_bz * dt; + + if (is_close_to_zero(delta_theta)) + { + /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): + integrate_runge_kutta_2(v_bx, omega_bz, dt); + } + else + { + /// Exact integration + const double heading_old = heading_; + const double R = delta_x_b / delta_theta; + heading_ += delta_theta; + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); + } +} + +void SteeringOdometry::reset_accumulators() +{ + linear_acc_ = rcppmath::RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = rcppmath::RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace steering_odometry diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml new file mode 100644 index 0000000000..d200d34961 --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -0,0 +1,17 @@ +test_steering_controllers_library: + ros__parameters: + + reference_timeout: 0.1 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp new file mode 100644 index 0000000000..f4e72361ac --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -0,0 +1,208 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_steering_controllers_library.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +class SteeringControllersLibraryTest +: public SteeringControllersLibraryFixture +{ +}; + +// checking if all interfaces, command, state and reference are exported as expected +TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +// Tests controller update_reference_from_subscribers and +// its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const double TEST_LINEAR_VELOCITY_X = 1.5; + const double TEST_LINEAR_VELOCITY_Y = 0.0; + const double TEST_ANGULAR_VELOCITY_Z = 0.3; + + std::shared_ptr msg = std::make_shared(); + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // case 1 position_feedback = false + controller_->params_.position_feedback = false; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + + // case 2 position_feedback = true + controller_->params_.position_feedback = true; + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp new file mode 100644 index 0000000000..d27d3a6afd --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -0,0 +1,344 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only + +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; +static constexpr double REAR_WHEEL_TRACK_ = 1.76868; +static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; +static constexpr double REAR_WHEELS_RADIUS_ = 0.45; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); + FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() + { + param_listener_ = std::make_shared(get_node()); + } + + controller_interface::CallbackReturn configure_odometry() + { + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SteeringControllersLibraryFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_steering_controllers_library") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_steering_controllers_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp new file mode 100644 index 0000000000..27ef11548a --- /dev/null +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -0,0 +1,378 @@ +// Copyright (c) 2023, Virtual Vehicle Research GmbH +// +// 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 "gmock/gmock.h" + +#include "steering_controllers_library/steering_odometry.hpp" + +TEST(TestSteeringOdometry, initialize) +{ + EXPECT_NO_THROW(steering_odometry::SteeringOdometry()); + + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 3.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + EXPECT_DOUBLE_EQ(odom.get_heading(), 0.); + EXPECT_DOUBLE_EQ(odom.get_x(), 0.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(2., 0., 0.5); + EXPECT_DOUBLE_EQ(odom.get_linear(), 2.); + EXPECT_DOUBLE_EQ(odom.get_x(), 1.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), 1.); + + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_GT(odom.get_y(), 0); // pos y, ie. left +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., -1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_LT(odom.get_y(), 0); // neg y ie. right +} + +TEST(TestSteeringOdometry, ackermann_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], cmd1[1]); // no steering + EXPECT_EQ(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // right (outer) < left (inner) + EXPECT_GT(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } + + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } +} + +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(vel_cmd_steered[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(vel_cmd_not_steered[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered_limited; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + vel_cmd_not_steered_limited = std::get<0>(cmd); // vel + EXPECT_GT(vel_cmd_not_steered_limited[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < vel_cmd_not_steered_limited.size(); ++i) + { + EXPECT_LT(vel_cmd_not_steered_limited[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + { + // larger error -> check min of scale + odom.update_from_position(0., M_PI, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered_limited now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered_limited[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } +} + +TEST(TestSteeringOdometry, bicycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } +} + +TEST(TestSteeringOdometry, tricycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0739bf6446..e309ed9315 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,151 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Remove empty on_shutdown() callbacks (backport `#1477 `_) (`#1482 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- +* Use the .hpp headers from `realtime_tools` package (backport `#1406 `_) (`#1427 `_) +* [CI] Add clang job and setup concurrency (backport `#1407 `_) (`#1418 `_) +* Contributors: mergify[bot] + +2.39.0 (2024-12-03) +------------------- +* TractionLimiter: Fix wrong input checks (`#1341 `_) (`#1366 `_) +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) +* Bump version of pre-commit hooks (`#1157 `_) (`#1158 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Fix usage of M_PI on Windows (`#1036 `_) (`#1037 `_) +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- +* Increase test coverage of interface configuration getters (`#856 `_) (`#865 `_) +* Contributors: mergify[bot] + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* [Formatting] enable ReflowComments to also use ColumnLimit on comments (`#628 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- * [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index c499bca983..46cd61acc3 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -1,70 +1,48 @@ -cmake_minimum_required(VERSION 3.8) -project(tricycle_controller) +cmake_minimum_required(VERSION 3.16) +project(tricycle_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(ackermann_msgs REQUIRED) - -add_library( - ${PROJECT_NAME} - SHARED - src/tricycle_controller.cpp - src/odometry.cpp - src/traction_limiter.cpp - src/steering_limiter.cpp -) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include -) - -ament_target_dependencies( - ${PROJECT_NAME} +set(THIS_PACKAGE_INCLUDE_DEPENDS ackermann_msgs builtin_interfaces controller_interface geometry_msgs hardware_interface nav_msgs - std_srvs pluginlib rclcpp rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs ) -pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib +add_library(tricycle_controller SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/traction_limiter.cpp + src/steering_limiter.cpp ) -install( - DIRECTORY - include/ - DESTINATION include +target_compile_features(tricycle_controller PUBLIC cxx_std_17) +target_include_directories(tricycle_controller PUBLIC + $ + $ ) +ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(tricycle_controller PRIVATE _USE_MATH_DEFINES) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -74,11 +52,9 @@ if(BUILD_TESTING) ament_add_gmock(test_tricycle_controller test/test_tricycle_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) - target_include_directories(test_tricycle_controller PRIVATE include) target_link_libraries(test_tricycle_controller - ${PROJECT_NAME} + tricycle_controller ) - ament_target_dependencies(test_tricycle_controller geometry_msgs hardware_interface @@ -90,29 +66,37 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock( - test_load_tricycle_controller + ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp ) - target_include_directories(test_load_tricycle_controller PRIVATE include) + target_link_libraries(test_load_tricycle_controller + tricycle_controller + ) ament_target_dependencies(test_load_tricycle_controller controller_manager ros2_control_test_assets ) + + ament_add_gmock(test_traction_limiter + test/test_traction_limiter.cpp) + target_link_libraries(test_traction_limiter + tricycle_controller + ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +install( + DIRECTORY include/ + DESTINATION include/tricycle_controller ) -ament_export_dependencies( - controller_interface - geometry_msgs - hardware_interface - rclcpp - rclcpp_lifecycle +install( + TARGETS + tricycle_controller + EXPORT export_tricycle_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_tricycle_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index a248bbec96..af5b69a985 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -1,19 +1,16 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/tricycle_controller/doc/userdoc.rst + .. _tricycle_controller_userdoc: tricycle_controller ===================== -Controller for mobile robots with tricycle drive. +Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. + Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. -Velocity commands ------------------ - -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. -Velocities on other components are ignored. - +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. Other features -------------- @@ -22,3 +19,13 @@ Other features Odometry publishing Velocity, acceleration and jerk limits Automatic stop after command timeout + + +ROS 2 Interfaces +------------------------ + +Subscribers +,,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index ea0bb16025..437f6cd4c2 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -28,14 +28,25 @@ class TractionLimiter public: /** * \brief Constructor + * + * Parameters are applied symmetrically for both directions, i.e., are applied + * to the absolute value of the corresponding quantity. + * + * \warning + * - Setting min_velocity: the robot can't stand still + * + * - Setting min_deceleration/min_acceleration: the robot can't move with constant velocity + * + * - Setting min_jerk: the robot can't move with constant acceleration + * * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] - * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 - * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3] + * \param [in] max_jerk Maximum jerk [m/s^3] */ TractionLimiter( double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index cef90d026a..9c8a1d5aca 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -35,11 +35,11 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" + #include "tricycle_controller/odometry.hpp" #include "tricycle_controller/steering_limiter.hpp" #include "tricycle_controller/traction_limiter.hpp" @@ -87,9 +87,6 @@ class TricycleController : public controller_interface::ControllerInterface TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; - protected: struct TractionHandle { diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 1681367c25..addb5dda26 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,32 +2,43 @@ tricycle_controller - 2.15.0 + 2.44.0 Controller for a tricycle drive mobile base + Bence Magyar - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Tony Najjar ament_cmake + ackermann_msgs + backward_ros + builtin_interfaces controller_interface geometry_msgs hardware_interface nav_msgs - std_srvs + pluginlib rclcpp rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs - ackermann_msgs - - pluginlib ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index 7865d4be71..4122fe3e76 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -45,13 +45,13 @@ TractionLimiter::TractionLimiter( if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; - if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_deceleration_)) min_deceleration_ = 0.0; if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; const std::string error = - "The positive limit will be applied to both directions. Setting different limits for positive " + " The positive limit will be applied to both directions. Setting different limits for positive " "and negative directions is not supported. Actuators are " "assumed to have the same constraints in both directions"; if (min_velocity_ < 0 || max_velocity_ < 0) @@ -59,19 +59,39 @@ TractionLimiter::TractionLimiter( throw std::invalid_argument("Velocity cannot be negative." + error); } + if (min_velocity_ > max_velocity_) + { + throw std::invalid_argument("Min velocity cannot be greater than max velocity."); + } + if (min_acceleration_ < 0 || max_acceleration_ < 0) { - throw std::invalid_argument("Acceleration cannot be negative." + error); + throw std::invalid_argument("Acceleration limits cannot be negative." + error); + } + + if (min_acceleration_ > max_acceleration_) + { + throw std::invalid_argument("Min acceleration cannot be greater than max acceleration."); } if (min_deceleration_ < 0 || max_deceleration_ < 0) { - throw std::invalid_argument("Deceleration cannot be negative." + error); + throw std::invalid_argument("Deceleration limits cannot be negative." + error); + } + + if (min_deceleration_ > max_deceleration_) + { + throw std::invalid_argument("Min deceleration cannot be greater than max deceleration."); } if (min_jerk_ < 0 || max_jerk_ < 0) { - throw std::invalid_argument("Jerk cannot be negative." + error); + throw std::invalid_argument("Jerk limits cannot be negative." + error); + } + + if (min_jerk_ > max_jerk_) + { + throw std::invalid_argument("Min jerk cannot be greater than max jerk."); } } @@ -103,7 +123,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) double dv_min; double dv_max; - if (abs(v) >= abs(v0)) + if (std::fabs(v) >= std::fabs(v0)) { dv_min = min_acceleration_ * dt; dv_max = max_acceleration_ * dt; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 4f4e22ec9d..48627e820b 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -67,7 +67,7 @@ CallbackReturn TricycleController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("odom_only_twist", odom_params_.odom_only_twist); - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); + auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); @@ -234,8 +234,8 @@ controller_interface::return_type TricycleController::update( AckermannDrive ackermann_command; // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel // speed (rad/s) - ackermann_command.speed = Ws_write; - ackermann_command.steering_angle = alpha_write; + ackermann_command.speed = static_cast(Ws_write); + ackermann_command.steering_angle = static_cast(alpha_write); previous_commands_.emplace(ackermann_command); // Publish ackermann command @@ -244,8 +244,8 @@ controller_interface::return_type TricycleController::update( auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel // speed (rad/s) - realtime_ackermann_command.speed = Ws_write; - realtime_ackermann_command.steering_angle = alpha_write; + realtime_ackermann_command.speed = static_cast(Ws_write); + realtime_ackermann_command.steering_angle = static_cast(alpha_write); realtime_ackermann_command_publisher_->unlockAndPublish(); } @@ -277,7 +277,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); + static_cast(get_node()->get_parameter("velocity_rolling_window_size").as_int())); odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); @@ -401,7 +401,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & }); } - // initialize odometry publisher and messasge + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = @@ -531,11 +531,6 @@ bool TricycleController::reset() return true; } -CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) -{ - return CallbackReturn::SUCCESS; -} - void TricycleController::halt() { traction_joint_[0].velocity_command.get().set_value(0.0); diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index d04b83ae27..229d014fe9 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -16,12 +16,14 @@ * Author: Tony Najjar */ -#include +#include #include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadTricycleController, load_controller) @@ -36,8 +38,9 @@ TEST(TestLoadTricycleController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController")); + ASSERT_NE( + cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), + nullptr); rclcpp::shutdown(); } diff --git a/tricycle_controller/test/test_traction_limiter.cpp b/tricycle_controller/test/test_traction_limiter.cpp new file mode 100644 index 0000000000..4b4a5be03a --- /dev/null +++ b/tricycle_controller/test/test_traction_limiter.cpp @@ -0,0 +1,547 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "tricycle_controller/traction_limiter.hpp" + +TEST(SpeedLimiterTest, testWrongParams) +{ + EXPECT_NO_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + )); + + // velocity + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + -20., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + -10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + 20., // min_velocity + 10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // acceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + -20., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + -10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + 20., // min_acceleration + 10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // deceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + -20., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + -10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + 20., // min_deceleration + 10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // jerk + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + -20. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + -10. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + 20., // min_jerk + 10. // max_jerk + ), + std::invalid_argument); + } +} + +TEST(SpeedLimiterTest, testNoLimits) +{ + tricycle_controller::TractionLimiter limiter; + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); +} + +TEST(SpeedLimiterTest, testVelocityLimits) +{ + tricycle_controller::TractionLimiter limiter(0.5, 1.0, 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); + { + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 1.0); + EXPECT_DOUBLE_EQ(limiting_factor, 0.1); + + v = 0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 0.1); + + // TODO(christophfroehlich): does this behavior make sense? + v = 0.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -1.0); + EXPECT_DOUBLE_EQ(limiting_factor, -1.0 / -10.0); + + v = -0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, -0.5 / -0.1); + } + + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testVelocityNoLimits) +{ + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testAccelerationLimits) +{ + { + // test max_acceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + { + // test min_acceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testDecelerationLimits) +{ + { + // test max_deceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit_acceleration(v, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } + { + // test min_deceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 9.9; + limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 9.0m.s-1 = 10 - 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 9.0); + + v = -9.9; + limiter.limit_acceleration(v, -10., 0.5); + // check if the robot speed is now -9.0m.s-1 = -10 + 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -9.0); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit(v, 10.0, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit(v, -10.0, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } +} + +TEST(SpeedLimiterTest, testJerkLimits) +{ + { + // test max_jerk + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 2.5m.s-1 = 5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -2.5m.s-1 = -5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } + { + // test min_jerk + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + // acceleration is limiting, not jerk + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..554663eaa5 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -39,6 +39,7 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +using testing::UnorderedElementsAre; class TestableTricycleController : public tricycle_controller::TricycleController { @@ -52,25 +53,20 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle } /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout - */ - bool wait_for_twist( + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } }; @@ -207,6 +203,20 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + state_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) @@ -318,7 +328,7 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..1333942859 --- /dev/null +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -0,0 +1,236 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tricycle_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.44.0 (2025-04-10) +------------------- +* Bump version of pre-commit hooks (backport `#1618 `_) (`#1620 `_) +* Contributors: mergify[bot] + +2.43.0 (2025-03-17) +------------------- + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- +* Update paths of GPL includes (backport `#1487 `_) (`#1493 `_) +* Contributors: Christoph Fröhlich + +2.41.0 (2025-01-13) +------------------- +* Fix typos in steering_controllers_lib (backport `#1464 `_) (`#1466 `_) +* Contributors: mergify[bot] + +2.40.0 (2025-01-01) +------------------- + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* 🚀 Add PID controller 🎉 (backport `#434 `_, `#975 `_, `#899 `_, `#1084 `_, `#951 `_) (`#1162 `_) +* Fix steering controllers library kinematics (`#1150 `_) (`#1194 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Contributors: mergify[bot] + +2.35.0 (2024-05-22) +------------------- +* Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) +* Contributors: mergify[bot] + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- +* Bump versions for release +* Let sphinx add parameter description to documentation (backport `#651 `_) (`#663 `_) +* Fix sphinx for steering odometry library/controllers (`#626 `_) (`#661 `_) +* Steering odometry library and controllers (backport `#484 `_) (`#624 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković, Reza Kermani, Denis Štogl + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..4c56d68185 --- /dev/null +++ b/tricycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(tricycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(tricycle_steering_controller_parameters + src/tricycle_steering_controller.yaml +) + +add_library( + tricycle_steering_controller + SHARED + src/tricycle_steering_controller.cpp +) +target_compile_features(tricycle_steering_controller PUBLIC cxx_std_17) +target_include_directories(tricycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(tricycle_steering_controller PUBLIC + tricycle_steering_controller_parameters) +ament_target_dependencies(tricycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(tricycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface tricycle_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_tricycle_steering_controller + test/test_load_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_tricycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller test/test_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml) + target_include_directories(test_tricycle_steering_controller PRIVATE include) + target_link_libraries(test_tricycle_steering_controller tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/tricycle_steering_controller +) + +install( + TARGETS tricycle_steering_controller tricycle_steering_controller_parameters + EXPORT export_tricycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_tricycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/tricycle_steering_controller/doc/userdoc.rst b/tricycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..618d7f04a7 --- /dev/null +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/tricycle_steering_controller/doc/userdoc.rst + +.. _tricycle_steering_controller_userdoc: + +tricycle_steering_controller +============================= + +This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +Additionally to the parameters of the :ref:`Steering Controller Library `, this controller adds the following parameters: + +.. generate_parameter_library_details:: ../src/tricycle_steering_controller.yaml diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp new file mode 100644 index 0000000000..7fa189a277 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "steering_controllers_library/steering_controllers_library.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller_parameters.hpp" +#include "tricycle_steering_controller/visibility_control.h" + +namespace tricycle_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_AXIS = 2; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_WHEEL = 2; + +static constexpr size_t NR_STATE_ITFS = 3; +static constexpr size_t NR_CMD_ITFS = 3; +static constexpr size_t NR_REF_ITFS = 2; + +class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + TricycleSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +protected: + std::shared_ptr tricycle_param_listener_; + tricycle_steering_controller::Params tricycle_params_; +}; +} // namespace tricycle_steering_controller + +#endif // TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..606b067ad8 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml new file mode 100644 index 0000000000..6da334dc62 --- /dev/null +++ b/tricycle_steering_controller/package.xml @@ -0,0 +1,45 @@ + + + + tricycle_steering_controller + 2.44.0 + Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + ament_cmake + + generate_parameter_library + + backward_ros + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp new file mode 100644 index 0000000000..03be6b88f0 --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -0,0 +1,97 @@ +// Copyright 2022 Pixel Robotics. +// +// 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 "tricycle_steering_controller/tricycle_steering_controller.hpp" + +namespace tricycle_steering_controller +{ +TricycleSteeringController::TricycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} +void TricycleSteeringController::initialize_implementation_parameter_listener() +{ + tricycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() +{ + tricycle_params_ = tricycle_param_listener_->get_params(); + + const double front_wheels_radius = tricycle_params_.front_wheels_radius; + const double rear_wheels_radius = tricycle_params_.rear_wheels_radius; + const double wheel_track = tricycle_params_.wheel_track; + const double wheelbase = tricycle_params_.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if ( + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); + } + } + } + return true; +} + +} // namespace tricycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + tricycle_steering_controller::TricycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml new file mode 100644 index 0000000000..6e5ae2b477 --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -0,0 +1,44 @@ +tricycle_steering_controller: + wheel_track: + { + type: double, + default_value: 0.0, + description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..0d04afdf38 --- /dev/null +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_tricycle_steering_controller", + "tricycle_steering_controller/TricycleSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..ad7f80607f --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -0,0 +1,278 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(TricycleSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(TricycleSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(TricycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp new file mode 100644 index 0000000000..9e04c30edf --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -0,0 +1,305 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using tricycle_steering_controller::STATE_STEER_AXIS; +using tricycle_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using tricycle_steering_controller::CMD_STEER_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableTricycleSteeringController +: public tricycle_steering_controller::TricycleSteeringController +{ + FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_interfaces); + FRIEND_TEST(TricycleSteeringControllerTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerTest, update_success); + FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(TricycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(TricycleSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return tricycle_steering_controller::TricycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, timeout); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class TricycleSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0]}; + + double wheelbase_ = 3.24644; + double wheel_track_ = 1.212121; + + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..6f2913aeb8 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -0,0 +1,102 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml new file mode 100644 index 0000000000..6bfb87a892 --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ea8b88002e --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/tricycle_steering_controller.xml b/tricycle_steering_controller/tricycle_steering_controller.xml new file mode 100644 index 0000000000..f0d5d85467 --- /dev/null +++ b/tricycle_steering_controller/tricycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Tricycle kinematics with two traction and one steering joint. + + + diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e47e52cdac..cae43ccd63 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,138 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.44.0 (2025-04-10) +------------------- + +2.43.0 (2025-03-17) +------------------- +* [ForwardCommandController] Fix the duplicate command interface types when reconfiguring the controller (backport `#1568 `_, `#1570 `_) (`#1569 `_) +* Contributors: mergify[bot] + +2.42.1 (2025-02-24) +------------------- + +2.42.0 (2025-02-17) +------------------- + +2.41.0 (2025-01-13) +------------------- + +2.40.0 (2025-01-01) +------------------- + +2.39.0 (2024-12-03) +------------------- +* Update maintainers and add url tags (`#1363 `_) (`#1364 `_) +* Contributors: mergify[bot] + +2.38.0 (2024-11-09) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) (`#1321 `_) +* Contributors: mergify[bot] + +2.37.3 (2024-09-11) +------------------- + +2.37.2 (2024-08-22) +------------------- + +2.37.1 (2024-08-14) +------------------- + +2.37.0 (2024-07-24) +------------------- +* Fix WaitSet issue in tests (backport `#1206 `_) (`#1211 `_) +* Contributors: mergify[bot] + +2.36.0 (2024-07-09) +------------------- + +2.35.0 (2024-05-22) +------------------- + +2.34.0 (2024-04-01) +------------------- + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* Contributors: Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 5f0560659a..3642ae1cb9 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -1,88 +1,75 @@ -cmake_minimum_required(VERSION 3.5) -project(velocity_controllers) +cmake_minimum_required(VERSION 3.16) +project(velocity_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(velocity_controllers SHARED src/joint_group_velocity_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - forward_command_controller - pluginlib - rclcpp +target_compile_features(velocity_controllers PUBLIC cxx_std_17) +target_include_directories(velocity_controllers PUBLIC + $ + $ ) +ament_target_dependencies(velocity_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(velocity_controllers PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_velocity_controller + ament_add_gmock(test_load_joint_group_velocity_controller test/test_load_joint_group_velocity_controller.cpp ) - target_include_directories(test_load_joint_group_velocity_controller PRIVATE include) + target_link_libraries(test_load_joint_group_velocity_controller + velocity_controllers + ) ament_target_dependencies(test_load_joint_group_velocity_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_velocity_controller + ament_add_gmock(test_joint_group_velocity_controller test/test_joint_group_velocity_controller.cpp ) - target_include_directories(test_joint_group_velocity_controller PRIVATE include) target_link_libraries(test_joint_group_velocity_controller - ${PROJECT_NAME} + velocity_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/velocity_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS + velocity_controllers + EXPORT export_velocity_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_velocity_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst index 62b6aa019f..469b975a97 100644 --- a/velocity_controllers/doc/userdoc.rst +++ b/velocity_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/velocity_controllers/doc/userdoc.rst + .. _velocity_controllers_userdoc: velocity_controllers @@ -5,7 +7,41 @@ velocity_controllers This is a collection of controllers that work using the "velocity" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the velocity on a certain joint to achieve a set position. -Hardware interface type ------------------------ +The package contains the following controllers: + +velocity_controllers/JointGroupVelocityController +------------------------------------------------- + +This is specialization of the :ref:`forward_command_controller ` that works using the "velocity" joint interface. + + +ROS 2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Topics +,,,,,,,,,,,,,,,,,, + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Joints' velocity commands + + +Parameters +,,,,,,,,,,,,,,,,,, +This controller overrides the interface parameter from :ref:`forward_command_controller `, and the +``joints`` parameter is the only one that is required. + +An example parameter file is given here + +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController -These controllers work with joints using the "velocity" command interface. + velocity_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 131bcd11f0..be8d6b097d 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,22 +1,32 @@ velocity_controllers - 2.15.0 + 2.44.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake + backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 43c1545461..112fdebce4 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -62,6 +51,7 @@ void JointGroupVelocityControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) @@ -99,12 +89,13 @@ TEST_F(JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails) // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); - // activate failed, 'acceleration' is not a registered interface for `joint1` + // activate should succeed now ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) @@ -203,10 +194,13 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..3078359028 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" using hardware_interface::CommandInterface; @@ -58,6 +59,7 @@ class JointGroupVelocityControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 36d5a8368d..e426349f96 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) @@ -32,8 +34,10 @@ TEST(TestLoadJointGroupVelocityController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_group_velocity_controller", "velocity_controllers/JointGroupVelocityController")); + ASSERT_NE( + cm.load_controller( + "test_joint_group_velocity_controller", "velocity_controllers/JointGroupVelocityController"), + nullptr); rclcpp::shutdown(); }